hige dura
Dependencies: FatFileSystem mbed
Diff: main.cpp
- Revision:
- 0:813b917360ac
- Child:
- 1:8048a8bcde59
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Apr 02 12:22:30 2012 +0000
@@ -0,0 +1,662 @@
+// dot-HR EKF
+
+#include "mbed.h"
+#include "SDFileSystem.h"
+#include "ADXL345_I2C.h"
+#include "ITG3200.h"
+#include "HMC5883L.h"
+
+Serial pc(USBTX, USBRX);
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
+ADXL345_I2C accelerometer(p9, p10);
+ITG3200 gyro(p9, p10);
+HMC5883L compass(p9, p10);
+Serial xbee(p13, p14);
+AnalogIn alt_in(p19);
+DigitalIn stop(p20);
+PwmOut ESC1(p21);
+PwmOut ESC2(p22);
+PwmOut ESC3(p23);
+PwmOut ESC4(p24);
+PwmOut ESC5(p25);
+PwmOut ESC6(p26);
+Serial navi(p28, p27); // tx, rx
+DigitalOut navi_flag(p29);
+
+#define pi 3.14159265
+
+#define N 3 // phi, the, psi
+#define M 6 // The numbers of rotors
+#define L 5 // The numbers of state quantities (optimal regulator)
+#define chan 4 // The numbers of channels
+#define chan_buf 17
+#define N_LWMA 100
+
+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);
+ navi.baud(57600);
+ xbee.baud(57600);
+
+ FILE *fp = fopen("/sd/sdtest.txt", "w");
+ if(fp == NULL) {
+ error("Could not open file for write\n");
+ while(1){ led2 = 1; wait(.5); led2 = 0; led3 = 0; wait(.5); led3 = 0; }
+ }
+
+ int correct_period = 1000;
+ int navi_period = 10;
+ int xbee_period = 100;
+ int correct_loop = -2000;
+ int navi_loop = 10;
+ int xbee_loop = 100;
+
+ double dt_wait = 0.0019;
+ //double dt_wait = 0.01;
+ 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, 0, 1};
+ double acc0[N] = {0, 0, 1};
+ double d_acc[N] = {0};
+ double gyro_deg[N] = {0};
+ double gyro_rad[N] = {0};
+ int mag[N] = {0};
+
+ // Measurement algorithm
+ double angle_acc[2] = {0};
+ double angle_deg[N] = {0};
+ double angle_rad[N] = {0};
+ double zLWMA[N] = {0};
+ double compass_rad = 0;
+ double compass_deg = 0;
+
+ // Gravity z
+ 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;
+
+ // Optimal regulator
+ double state[L];
+ // Gain
+ double Kr[M][L] = {{ 0, 5.774, 0, 2.288, 0.408},
+ { -5, 2.887,-1.982, 1.144,-0.408},
+ { -5,-2.887,-1.982,-1.144, 0.408},
+ { 0,-5.774, 0,-2.288,-0.408},
+ { 5,-2.887, 1.982,-1.144, 0.408},
+ { 5, 2.887, 1.982, 1.144,-0.408}};
+
+ double motor[M] = {0};
+ double motor_attitude[M] = {0}; // Pulth width by the attitude control
+ double thrust[M];
+
+ // Hovering puls width
+ //double hover = 0.0015;
+
+ // Coefficient of transfering Force to Puls
+ //double Cfp = 1/(1.4*pow(10.0, 6));
+ double Cfp = 2*pow(10.0, -7);
+ // Coefficient of transfering altitude to Puls
+ double Calt = 1/(1.4*pow(10.0, 5));
+
+ // Keeping altitude
+ double alt;
+ double alt_r = 1000; // Altitude reference 1m
+ double alt_keep;
+ double Kp_alt = 1;
+ double sensor_h = 300; // Hight of sensor
+
+ // Navigation
+ char command_buf1[chan_buf] = {0};
+ int command_buf2[chan_buf] = {0};
+ double command[chan] = {0};
+ int set_navi_flag = 0;
+
+ // *** Setting up sensors ***
+ //pc.printf("\r\n\r\nSetting up sensors\r\n");
+ xbee.printf("\r\n\r\nSetting up sensors\r\n");
+
+ // 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");
+ xbee.printf("didn't intitialize power control\n\r");
+ return 0;
+ }
+ // Full resolution, +/-16g, 4mg/LSB.
+ wait(.1);
+
+ if(accelerometer.setDataFormatControl(0x0B)){
+ //pc.printf("didn't set data format\n\r");
+ xbee.printf("didn't set data format\n\r");
+ return 0;
+ }
+ wait(.1);
+
+ // 3.2kHz data rate.
+ if(accelerometer.setDataRate(ADXL345_3200HZ)){
+ //pc.printf("didn't set data rate\n\r");
+ xbee.printf("didn't set data rate\n\r");
+ return 0;
+ }
+ wait(.1);
+
+ if(accelerometer.setPowerControl(MeasurementMode)) {
+ //pc.printf("didn't set the power control to measurement\n\r");
+ xbee.printf("didn't set the power control to measurement\n\r");
+ return 0;
+ }
+ wait(.1);
+
+ gyro.setLpBandwidth(LPFBW_42HZ);
+ compass.setDefault();
+ wait(.1); // Wait some time for all sensors (Need at least 5ms)
+
+ // *** Setting up motors ***
+ //pc.printf("Setting up motors\r\n");
+ xbee.printf("Setting up motors\r\n");
+ ESC1.period(0.016044); ESC2.period(0.016044); ESC3.period(0.016044); ESC4.period(0.016044); ESC5.period(0.016044); ESC6.period(0.016044);
+ // pulsewidth 0.0011~0.00195
+ ESC1.pulsewidth(0.001); ESC2.pulsewidth(0.001); ESC3.pulsewidth(0.001); ESC4.pulsewidth(0.001); ESC5.pulsewidth(0.001); ESC6.pulsewidth(0.001);
+ // Wait some time for ESC (about 10s)
+ wait(5);
+
+ // *** Setting up navigator ***
+ //pc.printf("Setting up navigator\r\n");
+ xbee.printf("Setting up navigator\r\n");
+ while( set_navi_flag==0 ){
+ navi_flag = 1;
+ for( int i=0;i<chan_buf;i++ ){
+ navi.scanf("%c",&command_buf1[i]);
+ if(command_buf1[i]=='a'){
+ set_navi_flag=1;
+ break;
+ }
+ }
+ navi_flag = 0;
+ wait(.1);
+ }
+
+ // *** 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;
+
+ led1 = 1; led2 = 1; led3 = 1; led4 = 1;
+
+ //pc.printf("Starting IMU unit\n\r");
+ xbee.printf("Starting IMU unit\n\r");
+ //pc.printf(" time phi the P Q R accX accY accZ alt T1 T2 T3 T4 T5 T6 thro roll pitch yaw\n\r");
+ xbee.printf(" time phi the P Q R accX accY accZ alt T1 T2 T3 T4 T5 T6 thro roll pitch yaw\n\r");
+
+ //while(1){
+ while( stop==0 ){
+
+ // Navigation
+ if( navi_loop>=navi_period ){
+
+ navi_flag = 1;
+
+ for( int i=0;i<chan_buf;i++ ){ navi.scanf("%c",&command_buf1[i]); }
+ for( int i=0;i<chan_buf;i++ ){
+ command_buf2[i] = (int)command_buf1[i]-48;
+ if( command_buf2[i]==-16 ){ command_buf2[i]=0; }
+ }
+ for( int i=0;i<chan;i++ ){ command[i] = 0.0001*command_buf2[1+i*4]+0.00001*command_buf2[2+i*4]+0.000001*command_buf2[3+i*4]+0.001; }
+
+ navi_flag = 0;
+ navi_loop = 0;
+
+ }
+ navi_loop++;
+ //command[0] = 0.0016; // Shiyougo comentout surukoto!!
+
+ // Updating accelerometer and compass
+ accelerometer.getOutput(bit_acc);
+ compass.readData(get_mag);
+ alt = alt_in/0.000074;
+
+ for( int i=0;i<N;i++ ){ acc0[i] = acc[i]; }
+
+ // Transfering units and Coordinate transform
+ acc[0] = (((int16_t)bit_acc[0]-calib_acc[0])-((int16_t)bit_acc[1]-calib_acc[1]))*0.004/sqrt(2.0);
+ acc[1] = (((int16_t)bit_acc[0]-calib_acc[0])+((int16_t)bit_acc[1]-calib_acc[1]))*0.004/sqrt(2.0);
+ acc[2] = ((int16_t)bit_acc[2]-calib_acc[2])*0.004+1;
+
+ gyro_deg[0] = ((gyro.getGyroX()-calib_gyro[0])-(gyro.getGyroY()-calib_gyro[1]))/14.375/sqrt(2.0);
+ gyro_deg[1] = (-(gyro.getGyroX()-calib_gyro[0])-(gyro.getGyroY()-calib_gyro[1]))/14.375/sqrt(2.0); // 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[0] = (int16_t)get_mag[0]; }
+
+ // Low pass filter for acc
+ //if( -0.05<acc[0] && acc[0]<0.05 ){ acc[0]=0; }
+ //if( -0.05<acc[1] && acc[1]<0.05 ){ acc[1]=0; }
+ //if( 0.95<acc[2] && acc[2]<1.05 ){ acc[2]=0; }
+ // Limitter for acc
+ if( acc[0]<-1 ){ acc[0]=-1; } if( 1<acc[0] ){ acc[0]=1; }
+ if( acc[1]<-1 ){ acc[1]=-1; } if( 1<acc[1] ){ acc[1]=1; }
+ if( acc[2]<-0 ){ acc[2]=-0; } if( 2<acc[2] ){ acc[2]=2; }
+
+ for( int i=0;i<N;i++ ){ d_acc[i] = acc[i]-acc0[i]; }
+ if( abs(d_acc[0])>=0.5 && abs(acc[0])>=0.3 ){ acc[0] = acc0[0]; }
+ if( abs(d_acc[1])>=0.5 && abs(acc[1])>=0.3 ){ acc[1] = acc0[1]; }
+ if( abs(d_acc[2])>=0.5 ){ acc[2] = acc0[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; }}
+ // Limitter for gyro
+ //for( int i=0;i<N;i++ ){if( gyro_deg[i]<-90 ){ gyro_deg[i]=-90; }} for( int i=0;i<N;i++ ){if( 90<gyro_deg[i] ){ gyro_deg[i]=90; }}
+
+ 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
+ //angle_acc[0] = asin(zLWMA[1])*180/pi;
+ //angle_acc[1] = asin(zLWMA[0])*180/pi;
+
+ // RK4 (Prediction)
+ p_RK4 = RK4(dt,angle_rad,gyro_rad);
+ for( int i=0;i<N;i++ ){ angle_rad[i] = *p_RK4; p_RK4 = p_RK4+1; }
+
+ // EKF (Correction)
+ EKF_predict(angle_rad,gyro_rad);
+ if ( correct_loop>=correct_period ){
+ p_EKF = EKF_correct(angle_rad,gyro_rad,zLWMA);
+ for ( int i=0;i<N;i++ ){ angle_rad[i] = *p_EKF; p_EKF = p_EKF+1; }
+ correct_loop = 0;
+ }
+ correct_loop++;
+
+ // Caribrating EKF
+ for( int i=0;i<N;i++ ){ angle_deg[i] = angle_rad[i]*180/pi; }
+
+ alt_keep = -Kp_alt*(alt-alt_r-sensor_h)*Calt;
+ //alt_keep = 0;
+
+ state[0] = angle_deg[0]; state[1] = angle_deg[1];
+ state[2] = gyro_deg[0]; state[3] = gyro_deg[1]; state[4] = gyro_deg[2];
+
+ for( int i=0;i<M;i++ ){
+ motor_attitude[i] = 0;
+ for( int j=0;j<L;j++ ){ motor_attitude[i] += -Kr[i][j]*state[j]; }
+ }
+
+ for( int i=0;i<M;i++ ){ motor[i] = command[0]+motor_attitude[i]*Cfp; }
+
+ // pulsewidth 0.0011~0.0019 (0.001~0.002?)
+ for( int i=0;i<M;i++ ){
+ if( motor[i]>0.00195 ){ motor[i]=0.00195; }
+ if( motor[i]<0.0011 ){ motor[i]=0.00105; }
+ if( command[0]<0.0011 ){ motor[i]=0.00105; }
+ }
+
+ ESC1.pulsewidth(motor[0]); ESC2.pulsewidth(motor[1]); ESC3.pulsewidth(motor[2]); ESC4.pulsewidth(motor[3]); ESC5.pulsewidth(motor[4]); ESC6.pulsewidth(motor[5]);
+
+ for( int i=0;i<M;i++ ){ thrust[i] = 630000.0*motor[i]*motor[i]-700.0*motor[i]; }
+
+ //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.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.3f, %7.3f, %7.3f, %7.1f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n\r", t, state[0], state[1], state[2], state[3], state[4], zLWMA[0], zLWMA[1], zLWMA[2], alt, motor[0]*1000, motor[1]*1000, motor[2]*1000, motor[3]*1000, motor[4]*1000, motor[5]*1000, command[0]*1000, command[1]*1000, command[2]*1000, command[3]*1000);
+ fprintf(fp,"%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %7.1f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n", t, state[0], state[1], state[2], state[3], state[4], acc[0], acc[1], acc[2], zLWMA[0], zLWMA[1], zLWMA[2], alt, motor[0]*1000, motor[1]*1000, motor[2]*1000, motor[3]*1000, motor[4]*1000, motor[5]*1000, command[0]*1000, command[1]*1000, command[2]*1000, command[3]*1000);
+ if( xbee_loop>=xbee_period ){
+ xbee.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %7.1f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n\r", t, state[0], state[1], state[2], state[3], state[4], acc[0], acc[1], acc[2], zLWMA[0], zLWMA[1], zLWMA[2], alt, motor[0]*1000, motor[1]*1000, motor[2]*1000, motor[3]*1000, motor[4]*1000, motor[5]*1000, command[0]*1000, command[1]*1000, command[2]*1000, command[3]*1000);
+ xbee_loop = 0;
+ }
+ xbee_loop++;
+ wait(dt_wait);
+ t += dt;
+
+ }
+
+ // pulsewidth 0.0011~0.00195
+ ESC1.pulsewidth(0.001); ESC2.pulsewidth(0.001); ESC3.pulsewidth(0.001); ESC4.pulsewidth(0.001); ESC5.pulsewidth(0.001); ESC6.pulsewidth(0.001);
+ led1 = 0; led2 = 0; led3 = 0; led4 = 0;
+ fclose(fp);
+
+}
+
+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* pzLWMA = 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 pzLWMA;
+
+}
+
+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("Don't touch... Calibrating now!!\n\r");
+ xbee.printf("Don'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");
+ xbee.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);
+ xbee.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;
+
+ if( calib_result[0]==0 && calib_result[1]==0 && calib_result[2]==0 ){
+ pc.printf("Accelerometer is not available.\r\n");
+ xbee.printf("Accelerometer is not available.\r\n");
+ }
+
+ for( int i=0;i<3;i++ ){
+ wait(.5);
+ led1 = 1; led2 = 1; led3 = 1; led4 = 1;
+ wait(.5);
+ led1 = 0; led2 = 0; led3 = 0; led4 = 0;
+ }
+
+ return p_calib_result;
+
+}
\ No newline at end of file