File content as of revision 11:b32b3d6be8d2:
// IMU for Sparkfun 9DOF Sensor Stick
#include "mbed.h"
#include "ADXL345_I2C.h"
#include "ITG3200.h"
#include "HMC5883L.h"
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
ADXL345_I2C accelerometer(p9, p10);
ITG3200 gyro(p9, p10);
HMC5883L compass(p9, p10);
Serial pc(USBTX, USBRX);
#define pi 3.14159265
#define N 3
#define N_LWMA 10
int preparing_acc();
double* calib();
double* RK4( double, double[N], double[N] );
double* func( double[N], double[N] );
double* LWMA( double[N] );
double* EKF_predict( double[N], double[N] );
double* EKF_correct( double[N], double[N], double[N] );
// 0 1 2
// [ accXn-1 accXn-2 ... ] 0
// zBuf = [ accYn-1 accYn-2 ... ] 1
// [ accZn-1 accZn-2 ... ] 2
double z_buf[N][N_LWMA] = {0}; // For LWMA
double P[N][N] = {{1,0,0},{0,1,0},{0,0,1}}; // For EKF
int main(){
pc.baud(921600);
int correct_loop = 0;
double dt_wait = 0.00514;
double dt = 0.01;
double t = 0;
int bit_acc[N] = {0}; // Buffer of the accelerometer
int get_mag[N] = {0}; // Buffer of the compass
// Calibration routine
double calib_acc[N] = {0};
double calib_gyro[N] = {0};
double compass_basis_rad = 0;
// Getting data
double acc[N] = {0};
double gyro_deg[N] = {0};
double gyro_rad[N] = {0};
int mag[N] = {0};
// Measurement algorithm
double ang_acc[2] = {0};
double ang_deg[N] = {0};
double ang_rad[N] = {0};
double zLWMA[N] = {0};
double compass_rad = 0;
double compass_deg = 0;
for( int i=0;i<N_LWMA;i++ ){ z_buf[2][i] = 1; }
double* p_calib;
double* p_RK4;
double* p_EKF;
double* p_zLWMA;
// *** Setting up sensors ***
int preparing_acc();
gyro.setLpBandwidth(LPFBW_42HZ);
compass.setDefault();
wait(0.1); // Wait some time for all sensors (Need at least 5ms)
// *** Calibration routine ***
p_calib = calib();
for( int i=0;i<3;i++ ){ calib_acc[i] = *p_calib; p_calib = p_calib+1; }
for( int i=3;i<6;i++ ){ calib_gyro[i-3] = *p_calib; p_calib = p_calib+1; }
compass_basis_rad = *p_calib;
pc.printf("Starting IMU unit\n\r");
pc.printf(" Time AccX AccY AccZ GyroX GyroY GyroZ MagX MagY MagZ\n\r");
while(1){
// Updating accelerometer and compass
accelerometer.getOutput(bit_acc);
compass.readData(get_mag);
// Transfering units (Acc[g], Gyro[deg/s], Compass[Ga])
acc[0] = ((int16_t)bit_acc[0]-calib_acc[0])*0.004;
acc[1] = ((int16_t)bit_acc[1]-calib_acc[1])*0.004;
acc[2] = ((int16_t)bit_acc[2]-calib_acc[2])*0.004+1;
gyro_deg[0] = (gyro.getGyroX()-calib_gyro[0])/14.375;
gyro_deg[1] = -(gyro.getGyroY()-calib_gyro[1])/14.375; // Modify the differencial of the sensor axis
gyro_deg[2] = -(gyro.getGyroZ()-calib_gyro[2])/14.375;
for( int i=0;i<N;i++ ){ mag[i] = (int16_t)get_mag[i]; }
// Low pass filter for acc
//for( int i=0;i<N;i++ ){ if( -0.05<acc[i] && acc[i]<0.05 ){ acc[i] = 0; } }
for( int i=0;i<N;i++ ){ if( acc[i]<-2 ){ acc[i] = -2; } }
for( int i=0;i<N;i++ ){ if( acc[i]>2 ){ acc[i] = 2; } }
// Low pass filter for gyro
//for( int i=0;i<N;i++ ){ if( -0.5<gyro_deg[i] && gyro_deg[i]<0.5 ){ gyro_deg[i] = 0; } }
for( int i=0;i<N;i++ ){ gyro_rad[i] = gyro_deg[i]*pi/180; }
// Compass yaw
/*compass_rad = (double)mag[1]/mag[0];
compass_rad = atan(compass_rad);
//compass_rad = compass_rad-compass_basis_rad;
compass_deg = compass_rad*180/pi;
*/
// LWMA (Observation)
p_zLWMA = LWMA(acc);
for( int i=0;i<N;i++ ){ zLWMA[i] = *p_zLWMA; p_zLWMA = p_zLWMA+1; }
// LWMA angle
ang_acc[0] = asin(zLWMA[1])*180/pi;
ang_acc[1] = asin(zLWMA[0])*180/pi;
// RK4 (Prediction)
p_RK4 = RK4(dt,ang_rad,gyro_rad);
for( int i=0;i<N;i++ ){ ang_rad[i] = *p_RK4; p_RK4 = p_RK4+1; }
// EKF (Correction)
EKF_predict(ang_rad,gyro_rad);
correct_loop++;
if (correct_loop>=10){
p_EKF = EKF_correct(ang_rad,gyro_rad,zLWMA);
for ( int i=0;i<N;i++ ){ ang_rad[i] = *p_EKF; p_EKF = p_EKF+1; }
correct_loop = 0;
}
for( int i=0;i<N;i++ ){ ang_deg[i] = ang_rad[i]*180/pi; }
//pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f, %5d, %5d, %5d\n\r", t, acc[0], acc[1], acc[2], gyro_deg[0], gyro_deg[1], gyro_deg[2], mag[0], mag[1], mag[2]);
//pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f\n\r", t, zLWMA[0], zLWMA[1], zLWMA[2], ang_acc[0], ang_acc[1]);
//pc.printf("%7.2f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f\n\r", t, ang_acc[0], ang_acc[1], ang_deg[0], ang_deg[1], ang_deg[2]);
pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f\n\r", t, gyro_deg[0], gyro_deg[1], gyro_deg[2], ang_deg[0], ang_deg[1], ang_deg[2]);
wait(dt_wait);
t += dt;
}
}
double* EKF_predict( double y[N], double x[N] ){
// x = F * x;
// P = F * P * F' + G * Q * G';
//double Q[N][N] = { {0.1, 0, 0}, {0, 0.1, 0}, {0, 0, 0.1} };
double Q[N][N] = { {0.00263, 0, 0}, {0, 0.00373, 0}, {0, 0, 0.00509} };
double Fjac[N][N] = {{x[1]*cos(y[0])*tan(y[1])-x[2]*sin(y[0])*tan(y[1]), x[1]*sin(y[0])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])/(cos(y[1])*cos(y[1])), 0}, {-x[1]*sin(y[0])-x[2]*cos(y[0]), 0, 0}, {x[1]*cos(y[0])/cos(y[1])-x[2]*sin(y[0])/cos(y[1]), x[1]*sin(y[0])*sin(y[1])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])*sin(y[1])/(cos(y[1])*cos(y[1])), 0}};
double Fjac_t[N][N] = {{x[1]*cos(y[0])*tan(y[1])-x[2]*sin(y[0])*tan(y[1]), -x[1]*sin(y[0])-x[2]*cos(y[0]), x[1]*cos(y[0])/cos(y[1])-x[2]*sin(y[0])/cos(y[1])}, {x[1]*sin(y[0])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])/(cos(y[1])*cos(y[1])), 0, x[1]*sin(y[0])*sin(y[1])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])*sin(y[1])/(cos(y[1])*cos(y[1]))}, {0, 0, 0}};
double Gjac[N][N] = {{1, sin(y[0])*tan(y[1]), cos(y[0])*tan(y[1])}, {0, cos(y[0]), -sin(y[0])}, {0, sin(y[0])/cos(y[1]), cos(y[0])/cos(y[1])}};
double Gjac_t[N][N] = {{1, 0, 0}, {sin(y[0])*tan(y[1]), cos(y[0]), sin(y[0])/cos(y[1])}, {cos(y[0])*tan(y[1]), -sin(y[0]), cos(y[0])/cos(y[1])}};
double FPF[N][N] = {0};
double GQG[N][N] = {0};
double FP[N][N] = {0};
double GQ[N][N] = {0};
for( int i=0;i<N;i++ ){
for( int j=0;j<N;j++ ){
for( int k=0;k<N;k++ ){
FP[i][j] += Fjac[i][k]*P[k][j];
GQ[i][j] += Gjac[i][k]*Q[k][j];
}
}
}
for( int i=0;i<N;i++ ){
for( int j=0;j<N;j++ ){
for( int k=0;k<N;k++ ){
FPF[i][j] += FP[i][k]*Fjac_t[k][j];
GQG[i][j] += GQ[i][k]*Gjac_t[k][j];
}
}
}
for( int i=0;i<N;i++ ){
for( int j=0;j<N;j++ ){
P[i][j] = FPF[i][j]+GQG[i][j];
}
}
return 0;
}
double* EKF_correct( double y[N], double x[N], double z[N] ){
// K = P * H' / ( H * P * H' + R )
// x = x + K * ( yobs(t) - H * x )
// P = P - K * H * P
//double R[N][N] = { {0.15, 0, 0}, {0, 0.15, 0}, {0, 0, 0.15} };
double R[N][N] = { {0.00015, 0, 0}, {0, 0.00016, 0}, {0, 0, 0.00032} };
double Hjac[N][N] = {{0, cos(y[1]), 0}, {cos(y[0]), 0, 0}, {-sin(y[0])*cos(y[1]), -cos(y[0])*sin(y[1]), 0}};
double Hjac_t[N][N] = {{0, cos(y[0]), -sin(y[0])*cos(y[1])}, {cos(y[1]), 0, -cos(y[0])*sin(y[1])}, {0, 0, 0}};
double K[N][N] = {0}; // Kalman gain
double K_deno[N][N] = {0}; // Denominator of the kalman gain
double det_K_deno_inv = 0;
double K_deno_inv[N][N] = {0};
double HPH[N][N] = {0};
double HP[N][N] = {0};
double PH[N][N] = {0};
double KHP[N][N] = {0};
double Hx[N] = {0};
double z_Hx[N] = {0};
double Kz_Hx[N] = {0};
double* py = y;
// Kalman gain
for( int i=0;i<N;i++ ){
for( int j=0;j<N;j++ ){
for( int k=0;k<N;k++ ){
HP[i][j] += Hjac[i][k]*P[k][j];
PH[i][j] += P[i][k]*Hjac_t[k][j];
}
}
}
for( int i=0;i<N;i++ ){
for( int j=0;j<N;j++ ){
for( int k=0;k<N;k++ ){
HPH[i][j] += HP[i][k]*Hjac_t[k][j];
}
}
}
for( int i=0;i<N;i++ ){
for( int j=0;j<N;j++ ){
K_deno[i][j] = HPH[i][j]+R[i][j];
}
}
// Inverce matrix
det_K_deno_inv = K_deno[0][0]*K_deno[1][1]*K_deno[2][2]+K_deno[1][0]*K_deno[2][1]*K_deno[0][2]+K_deno[2][0]*K_deno[0][1]*K_deno[1][2]-K_deno[0][0]*K_deno[2][1]*K_deno[1][2]-K_deno[2][0]*K_deno[1][1]*K_deno[0][2]-K_deno[1][0]*K_deno[0][1]*K_deno[2][2];
K_deno_inv[0][0] = (K_deno[1][1]*K_deno[2][2]-K_deno[1][2]*K_deno[2][1])/det_K_deno_inv;
K_deno_inv[0][1] = (K_deno[0][2]*K_deno[2][1]-K_deno[0][1]*K_deno[2][2])/det_K_deno_inv;
K_deno_inv[0][2] = (K_deno[0][1]*K_deno[1][2]-K_deno[0][2]*K_deno[1][1])/det_K_deno_inv;
K_deno_inv[1][0] = (K_deno[1][2]*K_deno[2][0]-K_deno[1][0]*K_deno[2][2])/det_K_deno_inv;
K_deno_inv[1][1] = (K_deno[0][0]*K_deno[2][2]-K_deno[0][2]*K_deno[2][0])/det_K_deno_inv;
K_deno_inv[1][2] = (K_deno[0][2]*K_deno[1][0]-K_deno[0][0]*K_deno[1][2])/det_K_deno_inv;
K_deno_inv[2][0] = (K_deno[1][0]*K_deno[2][1]-K_deno[1][1]*K_deno[2][0])/det_K_deno_inv;
K_deno_inv[2][1] = (K_deno[0][1]*K_deno[2][0]-K_deno[0][0]*K_deno[2][1])/det_K_deno_inv;
K_deno_inv[2][2] = (K_deno[0][0]*K_deno[1][1]-K_deno[0][1]*K_deno[1][0])/det_K_deno_inv;
for( int i=0;i<N;i++ ){
for( int j=0;j<N;j++ ){
for( int k=0;k<N;k++ ){
K[i][j] += PH[i][k]*K_deno_inv[k][j];
}
}
}
// Filtering
for( int i=0;i<N;i++ ){
for( int j=0;j<N;j++ ){
Hx[i] += Hjac[i][j]*y[j];
}
}
for( int i=0;i<N;i++ ){
z_Hx[i] = z[i]-Hx[i];
}
for( int i=0;i<N;i++ ){
for( int j=0;j<N;j++ ){
Kz_Hx[i] += K[i][j]*z_Hx[j];
}
}
for( int i=0;i<N;i++ ){
y[i] = y[i]+Kz_Hx[i];
}
// Covarience
for( int i=0;i<N;i++ ){
for( int j=0;j<N;j++ ){
for( int k=0;k<N;k++ ){
KHP[i][j] += K[i][k]*HP[k][j];
}
}
}
for( int i=0;i<N;i++ ){
for( int j=0;j<N;j++ ){
P[i][j] = P[i][j]-KHP[i][j];
}
}
return py;
}
double* LWMA( double z[N] ){
double zLWMA[N] = {0};
double zLWMA_num[N] = {0};
double zLWMA_den = 0;
double* p_zLWMA = zLWMA;
for( int i=1;i<N_LWMA;i++ ){
for( int j=0;j<N;j++ ){
z_buf[j][N_LWMA-i] = z_buf[j][N_LWMA-i-1];
}
}
for( int i=0;i<N;i++ ){
z_buf[i][0] = z[i];
}
for( int i=0;i<N_LWMA;i++ ){
for( int j=0;j<N;j++ ){
zLWMA_num[j] += (N_LWMA-i)*z_buf[j][i];
}
zLWMA_den += N_LWMA-i;
}
for( int i=0;i<N;i++ ){
zLWMA[i] = zLWMA_num[i]/zLWMA_den;
}
return p_zLWMA;
}
double* RK4( double dt, double y[N], double x[N] ){
double yBuf[N] = {0};
double k[N][4] = {0};
double* p_y = y;
double* pk1;
double* pk2;
double* pk3;
double* pk4;
for( int i=0;i<N;i++){ yBuf[i] = y[i]; }
pk1 = func (yBuf,x);
for( int i=0;i<N;i++ ){ k[i][0] = *pk1; pk1 = pk1+1; }
for( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][1]; }
pk2 = func (yBuf,x);
for( int i=0;i<N;i++ ){ k[i][1] = *pk2; pk2 = pk2+1; }
for( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][2]; }
pk3 = func (yBuf,x);
for( int i=0;i<N;i++ ){ k[i][2] = *pk3; pk3 = pk3+1; }
for( int i=0;i<N;i++){ yBuf[i] = y[i]+dt*k[i][3]; }
pk4 = func (yBuf,x);
for( int i=0;i<N;i++ ){ k[i][3] = *pk4; pk4 = pk4+1; }
for( int i=0;i<N;i++){ y[i] = y[i]+dt*(k[i][0]+2.0*k[i][1]+2.0*k[i][2]+k[i][3])/6.0; }
return p_y;
}
double* func( double y[N], double x[N] ){
double f[N] = {0};
double* p_f = f;
f[0] = x[0]+x[1]*sin(y[0])*tan(y[1])+x[2]*cos(y[0])*tan(y[1]);
f[1] = x[1]*cos(y[0])-x[2]*sin(y[0]);
f[2] = x[1]*sin(y[0])/cos(y[1])+x[2]*cos(y[0])/cos(y[1]);
return p_f;
}
double* calib(){
int calib_loop = 1000;
int bit_acc[N] = {0}; // Buffer of the accelerometer
int get_mag[N] = {0}; // Buffer of the compass
double calib_acc[N] = {0};
double calib_gyro_buf[N] = {0};
double calib_gyro[N] = {0};
double compass_basis_buf[N] = {0};
double compass_basis_rad = 0;
double calib_result[7] = {0};
double* p_calib_result = calib_result;
pc.printf("\n\r\n\rDon't touch... Calibrating now!!\n\r");
led1 = 1;
for( int i=0;i<calib_loop;i++ ){
accelerometer.getOutput(bit_acc);
compass.readData(get_mag);
calib_gyro_buf[0] = gyro.getGyroX();
calib_gyro_buf[1] = gyro.getGyroY();
calib_gyro_buf[2] = gyro.getGyroZ();
for( int j=0;j<N;j++ ){
calib_acc[j] += (int16_t)bit_acc[j];
calib_gyro[j] += calib_gyro_buf[j];
compass_basis_buf[j] += (int16_t)get_mag[j];
}
if( i>calib_loop*3/4 ){
led4 = 1;
}else if( i>calib_loop/2 ){
led3 = 1;
}else if( i>calib_loop/4 ){
led2 = 1;
}
}
for( int i=0;i<N;i++ ){
calib_acc[i] = calib_acc[i]/calib_loop;
calib_gyro[i] = calib_gyro[i]/calib_loop;
compass_basis_buf[i] = compass_basis_buf[i]/calib_loop;
}
compass_basis_rad = compass_basis_buf[1]/compass_basis_buf[0];
compass_basis_rad = atan(compass_basis_rad);
led1 = 0; led2 = 0; led3 = 0; led4 = 0;
pc.printf(" accX accY accZ gyroX gyroY gyroZ yaw_basis\n\r");
pc.printf("%6.1f, %6.1f, %6.1f, %6.1f, %6.1f, %6.1f, %6.1f\n\r",calib_acc[0],calib_acc[1],calib_acc[2],calib_gyro[0],calib_gyro[1],calib_gyro[2],compass_basis_rad*180/pi);
for( int i=0;i<3;i++ ){ calib_result[i] = calib_acc[i]; }
for( int i=3;i<6;i++ ){ calib_result[i] = calib_gyro[i-3]; }
calib_result[6] = compass_basis_rad;
for( int i=0;i<3;i++ ){
wait(0.5);
led1 = 1; led2 = 1; led3 = 1; led4 = 1;
wait(0.5);
led1 = 0; led2 = 0; led3 = 0; led4 = 0;
}
return p_calib_result;
}
int preparing_acc(){
// These are here to test whether any of the initialization fails. It will print the failure.
if (accelerometer.setPowerControl(0x00)){
pc.printf("didn't intitialize power control\n\r");
return 0;
}
// Full resolution, +/-16g, 4mg/LSB.
wait(.001);
if(accelerometer.setDataFormatControl(0x0B)){
pc.printf("didn't set data format\n\r");
return 0;
}
wait(.001);
// 3.2kHz data rate.
if(accelerometer.setDataRate(ADXL345_3200HZ)){
pc.printf("didn't set data rate\n\r");
return 0;
}
wait(.001);
if(accelerometer.setPowerControl(MeasurementMode)) {
pc.printf("didn't set the power control to measurement\n\r");
return 0;
}
wait(.001);
return 0;
}
hige dura