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 
/* 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]);
}