Go Down

### Topic: Measuring dynamic pitch (Read 1 time)previous topic - next topic

#### deviukk

##### Apr 07, 2011, 02:23 pm
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: [Select]
`#include <TinyGPS.h>// Accelerometer and gyroscope datafloat xkal, ykal, zkal, xvolt, yvolt, zvolt, gyrovolt, ygyro;float angleacc, anglegyro, prevAngle;const float accsens = 0.8; //166.67 count/gconst float accoff = 1.65;//343.75; // countconst float gyrooff = 1.35;const float gyrosens = 0.015;const float radToDegree = 180/3.14159265;const int calcount = 100;float first_time, time;float angle;float calx, caly, calz, calgyro;// Kalman filter dataconst 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 dataTinyGPS gps;float gpsdump(TinyGPS &gps);bool feedgps();float prevVel = 0;float accGPS;float dt, start1;// Averaging dataint 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;    angleacc = atan2(xkal,sqrt(pow(ykal,2) + pow(zkal,2)))*radToDegree;    //    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++){    calx = calx + analogRead(0);    caly = caly + analogRead(1);    calz = calz + analogRead(2);    calgyro = calgyro + analogRead(3);    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())  {    if (gps.encode(Serial.read()))      return true;  }  return false;} `

Some help?

#### MarkT

#1
##### Apr 07, 2011, 03:36 pm
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?
[ I will NOT respond to personal messages, I WILL delete them, use the forum please ]

#### johnwasser

#2
##### Apr 07, 2011, 03:42 pm
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.
Send Bitcoin tips to: 1G2qoGwMRXx8az71DVP1E81jShxtbSh5Hp

#### MarkT

#3
##### Apr 07, 2011, 03:59 pm
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.
[ I will NOT respond to personal messages, I WILL delete them, use the forum please ]

#### deviukk

#4
##### Apr 07, 2011, 06:22 pm
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?

#### MarkT

#5
##### Apr 08, 2011, 03:07 am
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.
[ I will NOT respond to personal messages, I WILL delete them, use the forum please ]

#### deviukk

#6
##### Apr 08, 2011, 11:15 am
This is how I implemented the angle from the gyroscope:

Code: [Select]
`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.

Go Up