Pages: [1]   Go Down
 Author Topic: Measuring dynamic pitch  (Read 502 times) 0 Members and 1 Guest are viewing this topic.
0
Offline
Newbie
Karma: 0
Posts: 35
Arduino rocks
 « on: April 07, 2011, 07:23:05 am » Bigger Smaller Reset

Hi all,

I'm trying to measure pitch in dynamic conditions with an accelerometer (MMA7260q) and a GPS (ls20031). In static conditions (while standing still) the pitch angle sensed by the accelerometer (after some filtering) is quite good and stable. But in dynamic conditions, the accelerometer is not only influenced by the gravity acceleration but also by the movement acceleration and so the measured angle is junk. So I thought I would measure the movement acceleration by a GPS and subtract it from the accelerometer reading. But this don't improve the angle.. I also have a gyroscope (IDG1215) that I can use..

Here is my code:
Code:
#include <TinyGPS.h>

// Accelerometer and gyroscope data
float xkal, ykal, zkal, xvolt, yvolt, zvolt, gyrovolt, ygyro;
float angleacc, anglegyro, prevAngle;
const float accsens = 0.8; //166.67 count/g
const float accoff = 1.65;//343.75; // count
const float gyrooff = 1.35;
const float gyrosens = 0.015;
const int calcount = 100;
float first_time, time;
float angle;
float calx, caly, calz, calgyro;

// Kalman filter data
const float Q = 1e-3;
const float R = 0.1;
float kal[3] = {337.59,337.59,501.27};
float P[3] = {10,10,10};
float kalmin[3];
float Pmin[3];
float K[3];

// GPS data
TinyGPS gps;
float gpsdump(TinyGPS &gps);
bool feedgps();
float prevVel = 0;
float accGPS;
float dt, start1;

// Averaging data
int teller = 0;
float gyrocount;
float accxcount, accycount, acczcount;

void setup()
{
Serial.begin(57600);
Serial.println("Calibrating...");
calibrate();
Serial.println("[OK]");
}

void loop()
{
boolean newdata = false;
unsigned long start = millis();
while(millis() - start < 100)
{
if (feedgps()){
newdata = true;
}
accxcount = accxcount + (analogRead(0) + calx);
accycount = accycount + (analogRead(1) + caly);
acczcount = acczcount + (analogRead(2) + calz);
//gyrocount = gyrocount + (analogRead(3) + calgyro);
teller++;
}
if(newdata){
accGPS = (float)gpsdump(gps);
getAccAngle(accGPS, gyrocount/teller, accxcount/teller, accycount/teller, acczcount/teller, 1);
}
gyrocount = accxcount = accycount = acczcount = teller = 0;
}

float getAccAngle(float acc2, float gyroavg, float accxavg, float accyavg, float acczavg, int pr){

// Time update
kalmin[0] = kal[0];
kalmin[1] = kal[1];
kalmin[2] = kal[2];

Pmin[0] = P[0] + Q;
Pmin[1] = P[1] + Q;
Pmin[2] = P[2] + Q;

// Measurement update
K[0] = Pmin[0]/(Pmin[0]+R);
K[1] = Pmin[1]/(Pmin[1]+R);
K[2] = Pmin[2]/(Pmin[2]+R);

float xmeas = accxavg - acc2;
float ymeas = accyavg;
float zmeas = acczavg;

kal[0] = kalmin[0] + K[0]*(xmeas-kalmin[0]) ; // Should be 337.59 flat
//Serial.println(kal[0]);
kal[1] = kalmin[1] + K[1]*(ymeas-kalmin[1]) ; // Should be 337.59 flat
kal[2] = kalmin[2] + K[2]*(zmeas-kalmin[2]) ; // Should be 501.27 flat

P[0] = (1-K[0])*Pmin[0];
P[1] = (1-K[1])*Pmin[1];
P[2] = (1-K[2])*Pmin[2];

xvolt = kal[0]*5/1023; // Should be 1.65V flat
yvolt = kal[1]*5/1023; // Should be 1.65V flat
zvolt = kal[2]*5/1023; // Should be 2.45V fla

xkal = (xvolt-accoff)/accsens;
//Serial.println(xkal);
ykal = (yvolt-accoff)/accsens;
zkal = (zvolt-accoff)/accsens;

//    gyrovolt = gyroavg*5/1023;
//    ygyro = -((gyrovolt-gyrooff)/gyrosens);
//    if (ygyro > -1.2 && ygyro < 1.2){
//      ygyro = 0;
//    }
//    time = (millis()-first_time)/1000;
//    first_time = millis();
//
//    anglegyro = anglegyro + ygyro*time;
//    angle = 0.98*(angle + ygyro*time) + 0.02*angleacc;

// Printing the values

Serial.println(angleacc);

}

void calibrate(){
for (int i = 0; i < calcount; i++){
delay(50);
}
calx = 337.59 - calx/calcount;
caly = 337.59 - caly/calcount;
calz = 501.27 - calz/calcount;
calgyro = 276.21 - calgyro/calcount;
}

float gpsdump(TinyGPS &gps)
{
float flat, flon;
unsigned long fix_age;

gps.f_get_position(&flat, &flon, &fix_age);
float falt = gps.f_altitude();
float speedms = gps.f_speed_mps();

//Serial.print("Latitude: "); Serial.println(flat,7);
//Serial.print("Longitude: "); Serial.println(flon,7);
//Serial.print("Altitude: "); Serial.println(falt, 5);
//Serial.print("Speed m/s: "); Serial.println(speedms, 5);

dt = (float)(millis()-start1)/1000;
start1 = (float)(millis());
float temp = (float)(speedms-prevVel);
prevVel = speedms;

//Serial.print("Acceleration: ");Serial.println(temp/dt,5);

return (float)(temp/dt);

}

bool feedgps()
{
while (Serial.available())
{

return true;
}
return false;
}

Some help?
 Logged

0
Offline
Tesla Member
Karma: 71
Posts: 6596
Arduino rocks
 « Reply #1 on: April 07, 2011, 08:36:10 am » Bigger Smaller Reset

For angles a gyroscope will work much much better (at least for fast changes - on a longer time scale the accelerometer is more useful).  Google "Kalman filter" to find out more about inertial navigation techniques?
 Logged

Massachusetts, USA
Offline
Tesla Member
Karma: 96
Posts: 6335
 « Reply #2 on: April 07, 2011, 08:42:42 am » Bigger Smaller Reset

Given only the magnitudes of two vectors and their vector sum I don't think there is a unique solution that will get you the directions of the two vectors.

Perhaps if you calculated the acceleration vector from the GPS data (not just the magnitude of the acceleration but the actual direction of acceleration) you could subtract that from the accelerometer readings to get your current roll and pitch angles.
 Logged

0
Offline
Tesla Member
Karma: 71
Posts: 6596
Arduino rocks
 « Reply #3 on: April 07, 2011, 08:59:57 am » Bigger Smaller Reset

The problem with GPS and accelerometers is that they are both noisy signals - in particular accelerometers are ambiguous if rotations are happening - the only way to correct for that is to have a full 6-degrees-of-freedom inertial platform.  If you are only interested in pitch then a single axis rate gyro can give you that, but only if the drift is corrected by a low-pass filtered reference vertical from an accelerometer (provided the motion is not too wild).  Modern silicon gyros drift on timescales of many seconds to tens of seconds so the correction signal can be very low bandwidth.

GPS signal is firstly not updated nearly fast enough to compare with an accelerometer, and also has an unknown delay to compensate for - its good for correcting position drift but not so good for giving timely velocity or acceleration.
 Logged

0
Offline
Newbie
Karma: 0
Posts: 35
Arduino rocks
 « Reply #4 on: April 07, 2011, 11:22:07 am » Bigger Smaller Reset

Thanks for all the reactions!

If you are only interested in pitch then a single axis rate gyro can give you that, but only if the drift is corrected by a low-pass filtered reference vertical from an accelerometer (provided the motion is not too wild).

How would you best implement such a system where the gyro is compensated by the accelerometer?

So, the 1-axis gyro and the 3-axis accelerometer would be enough? I tried to calculate the angle with the gyro and I tested it under static conditions (so the commented lines in the code). But this angle was far from the exact one... Is the code wrong, or is my gyroscope not good for this application?

 Logged

0
Offline
Tesla Member
Karma: 71
Posts: 6596
Arduino rocks
 « Reply #5 on: April 07, 2011, 08:07:25 pm » Bigger Smaller Reset

You need to look up Kalman filtering for combining signals from the two sensors.

The rate gyro output needs to be integrated to give an angle, that angle value (if the zero offset is correct) will only drift quite slowly.  Its important you measure the stationary 'zero' value by averaging several values to get more resolution in order to get the lowest drift.
 Logged

0
Offline
Newbie
Karma: 0
Posts: 35
Arduino rocks
 « Reply #6 on: April 08, 2011, 04:15:40 am » Bigger Smaller Reset

This is how I implemented the angle from the gyroscope:

Code:
gyrovolt = gyroavg*5/1023;
ygyro = -((gyrovolt-gyrooff)/gyrosens);
if (ygyro > -1.2 && ygyro < 1.2){
ygyro = 0;
}
time = (millis()-first_time)/1000;
first_time = millis();
anglegyro = anglegyro + ygyro*time;

I'll look up the Kalman filtering.
 Logged

 Pages: [1]   Go Up