Measuring dynamic pitch

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:

#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 float radToDegree = 180/3.14159265;
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;

    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?

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?

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.

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.

Thanks for all the reactions!

MarkT:
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?

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.

This is how I implemented the angle from the gyroscope:

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.