Inertial navigation using MPU9150

Hi all,
I’m trying to write an inertial navigation algorithm using the MPU9150, and can’t figure out why it wouldn’t work.
The attitude portion actualy works well better then I excpected, but the velocity part just gives garbage (maybe the acceleration too, but it’s pretty hard figuring what acceleration to expect, unless when the IMU is stationary- in which case the acceleration values do make sense)

can anyone spot the bug I’m missing? (printResults is missing from this message as it makes it too long for the forum, but it is tested and working as excpected)
Thanks a lot for any input :slight_smile:

/*  CONNECTIONS:
 *  MPU9150->arduino
 *  VCC->3.3V
 *  GND->GND
 *  SCL->A5
 *  SDA->A4
 */


#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "MatrixMath.h"

MPU6050 IMU;

int16_t acc_raw[3] ; //the raw values as received from the accelerometer
int16_t gyro_raw[3]; //the raw values as received from the gyro
int16_t mag_raw[3] ; //the raw values as received from the magnetometer

float acc_in[3] ; //the acceleration sensed by the accelerometer [m/s^2]
float gyro_in[3]; //the angular velocity sensed by the gyro [rad/s]
float mag_in[3] ; //for future use

float avgGyroZero[3]={0}; //the averege input from the gyros when standing still
float avgAccZero[3] ={0}; //the averege input from the accelerometers when standing still

float att[3]={0}; //attitude     [rad]   (psi,theta,phi) or (yaw,pitch,roll)
float acc[3]={0}; //acceleration [m/s^2] (North, East,Down)
float vel[3]={0}; //velocity     [m/s]   (North, East,Down)
float pos[3]={0}; //position     [m]     (North, East,Down)

int   lastTime=0;//[ms]
int   now     =0;//[ms]
float dt      =0;//[s]

void setup() {
    Serial.begin(250000);
    
    Wire.begin();

    IMU.initialize();

    //taking 1000 samples to determine the IMU's zero values
    int tempGyro[3]={0};
    int tempAcc[3] ={0};
    for(double i=1;i<1000;i++)
    {
      IMU.getRotation(&tempGyro[0], &tempGyro[1], &tempGyro[2]);
      IMU.getAcceleration(&tempAcc[0], &tempAcc[1], &tempAcc[2]);
      tempAcc[2]=tempAcc[2]-16384;
      for(int j=0;j<3;j++)
      {
        avgGyroZero[j]=((i-1)/i)*avgGyroZero[j]+(1/i)*float(tempGyro[j]);
        avgAccZero[j]=((i-1)/i)*avgAccZero[j]+(1/i)*float(tempAcc[j]);
      }
    }

    lastTime=millis();
}

void loop() {

    getIMUinput();
    StrapDown();
    printResults();

}

void getIMUinput()
{
    /*
     * This function is used to get data from the IMU (inertial measurement unit), 
     * and transform the data to meaningful values (i.e. from 16 bit counts to rad/sec angular velocity)
     * It also flips Y and Z directions in order to use the common aerospace axes system
     */
    IMU.getMotion9(&acc_raw[0], &acc_raw[1], &acc_raw[2], &gyro_raw[0], &gyro_raw[1], &gyro_raw[2], &mag_raw[0], &mag_raw[1], &mag_raw[2]);
    
    now=millis();
    dt=float(now-lastTime)/1000;
    lastTime=now;
    
    for(int i=0;i<3;i++)
    {
      acc_in[i]=(float(acc_raw[i])-avgAccZero[i])*9.83/16384; //transform accelerometer input to m/s^2
      gyro_in[i]=((float(gyro_raw[i])-avgGyroZero[i])/131/180*3.141)*dt; //transform gyro input to rad/s
    }
      acc_in[1]=-acc_in[1];  //
      acc_in[2]=-acc_in[2];  // The IMU's output assumes y points to the right and Z points up
      gyro_in[1]=-gyro_in[1];// The axis system common in aerospace assumes y points to the right and Z points down
      gyro_in[2]=-gyro_in[2];//
}

void StrapDown()
  {
    /*
     * This function uses the input from the IMU to calculate the IMU's: 
     * Attitude (using euler angles) 
     * Acceleration (in m/s^2, in North-East-Down axes, using initial X direction as North)
     * Velocity (in m/s, in North-East-Down axes, using initial X direction as North))
     * position (in m, in North-East-Down axes, using initial X direction as North, respective to initial position)
     */
     
    /********************************************/
    /******************ATTITUDE******************/
    /********************************************/
    float sp=sin(att[0]);
    float cp=cos(att[0]);
    float st=sin(att[1]);
    float ct=cos(att[1]);
    float sf=sin(att[2]);
    float cf=cos(att[2]);

    float M[3][3] = {{-st  ,0  ,1},
                     {ct*sf,cf ,0},
                     {ct*cf,-sf,0}};
                     
    float Minv[3][3]={0};
    Matrix.Copy(&M[0][0],3,3,&Minv[0][0]);
    Matrix.Invert(&Minv[0][0],3);

    float Cl2m[3][3]={{cp*ct         , ct*sp         , -st  },
                      {sf*st*cp-cf*sp, sf*st*sp+cf*cp, sf*ct},
                      {sf*sp+cf*st*cp, cf*st*sp-sf*cp, cf*ct}};
    
    float attIncrement[3];
    Matrix.Multiply(&Minv[0][0],&gyro_in[0],3,3,1,&attIncrement[0]);
    
    float newAtt[3];
    Matrix.Add(&att[0],&attIncrement[0],3,1,&newAtt[0]);
    Matrix.Copy(&newAtt[0],1,3,&att[0]);

    /************************************************/
    /******************ACCELERATION******************/
    /************************************************/
    float Cm2l[3][3]={0};
    Matrix.Transpose(&Cl2m[0][0],3,3,&Cm2l[0][0]); //Cm2l is a rotation matrix to transform vectors from IMU axes to LLLN (local north local level) axes

    float temp[3];
    Matrix.Multiply(&Cm2l[0][0],&acc_in[0],3,3,1,&temp[0]);
    
    float gVec[3]={0,0,9.83}; //a vector for gravitational acceleration in LLLN axes
    Matrix.Add(&temp[0],&gVec[0],3,1,&acc[0]); // a=g+Cm2l*acc_in

    /********************************************/
    /******************VELOCITY******************/
    /********************************************/
    float velIncrement[3]={0};
    for(int i=0;i<3;i++)
    {
      velIncrement[i]=acc[i]*dt;
    }
    float newVel[3];
    Matrix.Add(&vel[0],&velIncrement[0],3,1,&newVel[0]);
    Matrix.Copy(&newVel[0],3,1,&vel[0]);
    
    }

Your efforts are doomed to failure, because consumer grade sensors are currently too inaccurate and noisy to be useful for inertial navigation. You can read more about the problems here.

jremington:
Your efforts are doomed to failure, because consumer grade sensors are currently too inaccurate and noisy to be useful for inertial navigation. You can read more about the problems here.

Thanks for replying!

I should have mantioned that I'm aware of the inherent problems of cheap sensors, but:
a. I'm not planning on using dead-reckoning (I'll use a Kalman filter to update measurements using either a GPS or a set of tracking cameras)
b. Even without GPS updating I should get a few seconds of good data, but I can't even get that..

You might take a look at RTIMUlib, which will subtract the estimated gravity vector and return the remaining "local" acceleration.

It is simple to use and seems to be state of the art.

jremington:
You might take a look at RTIMUlib, which will subtract the estimated gravity vector and return the remaining "local" acceleration.

It is simple to use and seems to be state of the art.

I'll give it a try, thanks!