hige dura
/
IMU
main.cpp@10:3cfc137e23e8, 2012-03-14 (annotated)
- Committer:
- higedura
- Date:
- Wed Mar 14 20:39:55 2012 +0000
- Revision:
- 10:3cfc137e23e8
- Parent:
- 9:69ec742957ea
- Child:
- 11:b32b3d6be8d2
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
higedura | 2:78c3d0598819 | 1 | // IMU for Sparkfun 9DOF Sensor Stick |
higedura | 2:78c3d0598819 | 2 | |
higedura | 6:2f8a7136e326 | 3 | #include "mbed.h" |
higedura | 2:78c3d0598819 | 4 | #include "ADXL345_I2C.h" |
higedura | 2:78c3d0598819 | 5 | #include "ITG3200.h" |
higedura | 2:78c3d0598819 | 6 | #include "HMC5883L.h" |
higedura | 2:78c3d0598819 | 7 | |
higedura | 2:78c3d0598819 | 8 | DigitalOut led1(LED1); |
higedura | 2:78c3d0598819 | 9 | DigitalOut led2(LED2); |
higedura | 2:78c3d0598819 | 10 | DigitalOut led3(LED3); |
higedura | 2:78c3d0598819 | 11 | DigitalOut led4(LED4); |
higedura | 2:78c3d0598819 | 12 | ADXL345_I2C accelerometer(p9, p10); |
higedura | 2:78c3d0598819 | 13 | ITG3200 gyro(p9, p10); |
higedura | 2:78c3d0598819 | 14 | HMC5883L compass(p9, p10); |
higedura | 2:78c3d0598819 | 15 | Serial pc(USBTX, USBRX); |
higedura | 2:78c3d0598819 | 16 | |
higedura | 2:78c3d0598819 | 17 | #define pi 3.14159265 |
higedura | 2:78c3d0598819 | 18 | |
higedura | 2:78c3d0598819 | 19 | #define N 3 |
higedura | 2:78c3d0598819 | 20 | #define N_LWMA 10 |
higedura | 2:78c3d0598819 | 21 | |
higedura | 6:2f8a7136e326 | 22 | int preparing_acc(); |
higedura | 6:2f8a7136e326 | 23 | double* calib(); |
higedura | 2:78c3d0598819 | 24 | double* RK4( double, double[N], double[N] ); |
higedura | 2:78c3d0598819 | 25 | double* func( double[N], double[N] ); |
higedura | 2:78c3d0598819 | 26 | double* LWMA( double[N] ); |
higedura | 2:78c3d0598819 | 27 | double* EKF_predict( double[N], double[N] ); |
higedura | 2:78c3d0598819 | 28 | double* EKF_correct( double[N], double[N], double[N] ); |
higedura | 2:78c3d0598819 | 29 | |
higedura | 2:78c3d0598819 | 30 | // 0 1 2 |
higedura | 2:78c3d0598819 | 31 | // [ accXn-1 accXn-2 ... ] 0 |
higedura | 2:78c3d0598819 | 32 | // zBuf = [ accYn-1 accYn-2 ... ] 1 |
higedura | 2:78c3d0598819 | 33 | // [ accZn-1 accZn-2 ... ] 2 |
higedura | 2:78c3d0598819 | 34 | double z_buf[N][N_LWMA] = {0}; // For LWMA |
higedura | 2:78c3d0598819 | 35 | |
higedura | 2:78c3d0598819 | 36 | double P[N][N] = {{1,0,0},{0,1,0},{0,0,1}}; // For EKF |
higedura | 2:78c3d0598819 | 37 | |
higedura | 2:78c3d0598819 | 38 | int main(){ |
higedura | 2:78c3d0598819 | 39 | |
higedura | 2:78c3d0598819 | 40 | pc.baud(921600); |
higedura | 2:78c3d0598819 | 41 | |
higedura | 2:78c3d0598819 | 42 | int correct_loop = 0; |
higedura | 2:78c3d0598819 | 43 | |
higedura | 4:41289b278f12 | 44 | double dt_wait = 0.00514; |
higedura | 4:41289b278f12 | 45 | double dt = 0.01; |
higedura | 2:78c3d0598819 | 46 | double t = 0; |
higedura | 2:78c3d0598819 | 47 | |
higedura | 2:78c3d0598819 | 48 | int bit_acc[N] = {0}; // Buffer of the accelerometer |
higedura | 2:78c3d0598819 | 49 | int get_mag[N] = {0}; // Buffer of the compass |
higedura | 2:78c3d0598819 | 50 | |
higedura | 3:9f3e1a43f68f | 51 | // Calibration routine |
higedura | 2:78c3d0598819 | 52 | double calib_acc[N] = {0}; |
higedura | 2:78c3d0598819 | 53 | double calib_gyro[N] = {0}; |
higedura | 2:78c3d0598819 | 54 | double compass_basis_rad = 0; |
higedura | 2:78c3d0598819 | 55 | |
higedura | 3:9f3e1a43f68f | 56 | // Getting data |
higedura | 2:78c3d0598819 | 57 | double acc[N] = {0}; |
higedura | 2:78c3d0598819 | 58 | double gyro_deg[N] = {0}; |
higedura | 2:78c3d0598819 | 59 | double gyro_rad[N] = {0}; |
higedura | 2:78c3d0598819 | 60 | int mag[N] = {0}; |
higedura | 2:78c3d0598819 | 61 | |
higedura | 3:9f3e1a43f68f | 62 | // Measurement algorithm |
higedura | 2:78c3d0598819 | 63 | double ang_acc[2] = {0}; |
higedura | 2:78c3d0598819 | 64 | double ang_deg[N] = {0}; |
higedura | 2:78c3d0598819 | 65 | double ang_rad[N] = {0}; |
higedura | 2:78c3d0598819 | 66 | double zLWMA[N] = {0}; |
higedura | 2:78c3d0598819 | 67 | double compass_rad = 0; |
higedura | 2:78c3d0598819 | 68 | double compass_deg = 0; |
higedura | 2:78c3d0598819 | 69 | |
higedura | 8:56fb656dcef6 | 70 | for( int i=0;i<N_LWMA;i++ ){ z_buf[2][i] = 1; } |
higedura | 2:78c3d0598819 | 71 | |
higedura | 6:2f8a7136e326 | 72 | double* p_calib; |
higedura | 6:2f8a7136e326 | 73 | double* p_RK4; |
higedura | 6:2f8a7136e326 | 74 | double* p_EKF; |
higedura | 6:2f8a7136e326 | 75 | double* p_zLWMA; |
higedura | 2:78c3d0598819 | 76 | |
higedura | 6:2f8a7136e326 | 77 | // *** Setting up sensors *** |
higedura | 6:2f8a7136e326 | 78 | int preparing_acc(); |
higedura | 2:78c3d0598819 | 79 | gyro.setLpBandwidth(LPFBW_42HZ); |
higedura | 2:78c3d0598819 | 80 | compass.setDefault(); |
higedura | 6:2f8a7136e326 | 81 | wait(0.1); // Wait some time for all sensors (Need at least 5ms) |
higedura | 2:78c3d0598819 | 82 | |
higedura | 2:78c3d0598819 | 83 | // *** Calibration routine *** |
higedura | 6:2f8a7136e326 | 84 | p_calib = calib(); |
higedura | 6:2f8a7136e326 | 85 | for( int i=0;i<3;i++ ){ calib_acc[i] = *p_calib; p_calib = p_calib+1; } |
higedura | 6:2f8a7136e326 | 86 | for( int i=3;i<6;i++ ){ calib_gyro[i-3] = *p_calib; p_calib = p_calib+1; } |
higedura | 6:2f8a7136e326 | 87 | compass_basis_rad = *p_calib; |
higedura | 6:2f8a7136e326 | 88 | |
higedura | 10:3cfc137e23e8 | 89 | pc.printf("Starting IMU unit\n\r"); |
higedura | 2:78c3d0598819 | 90 | pc.printf(" Time AccX AccY AccZ GyroX GyroY GyroZ MagX MagY MagZ\n\r"); |
higedura | 2:78c3d0598819 | 91 | while(1){ |
higedura | 2:78c3d0598819 | 92 | |
higedura | 2:78c3d0598819 | 93 | // Updating accelerometer and compass |
higedura | 2:78c3d0598819 | 94 | accelerometer.getOutput(bit_acc); |
higedura | 2:78c3d0598819 | 95 | compass.readData(get_mag); |
higedura | 2:78c3d0598819 | 96 | |
higedura | 2:78c3d0598819 | 97 | // Transfering units (Acc[g], Gyro[deg/s], Compass[Ga]) |
higedura | 9:69ec742957ea | 98 | acc[0] = ((int16_t)bit_acc[0]-calib_acc[0])*0.004; |
higedura | 2:78c3d0598819 | 99 | acc[1] = ((int16_t)bit_acc[1]-calib_acc[1])*0.004; |
higedura | 2:78c3d0598819 | 100 | acc[2] = ((int16_t)bit_acc[2]-calib_acc[2])*0.004+1; |
higedura | 2:78c3d0598819 | 101 | |
higedura | 6:2f8a7136e326 | 102 | gyro_deg[0] = (gyro.getGyroX()-calib_gyro[0])/14.375; |
higedura | 9:69ec742957ea | 103 | gyro_deg[1] = -(gyro.getGyroY()-calib_gyro[1])/14.375; // Modify the differencial of the sensor axis |
higedura | 9:69ec742957ea | 104 | gyro_deg[2] = -(gyro.getGyroZ()-calib_gyro[2])/14.375; |
higedura | 6:2f8a7136e326 | 105 | |
higedura | 6:2f8a7136e326 | 106 | for( int i=0;i<N;i++ ){ mag[i] = (int16_t)get_mag[i]; } |
higedura | 2:78c3d0598819 | 107 | |
higedura | 2:78c3d0598819 | 108 | // Low pass filter for acc |
higedura | 2:78c3d0598819 | 109 | //for( int i=0;i<N;i++ ){ if( -0.05<acc[i] && acc[i]<0.05 ){ acc[i] = 0; } } |
higedura | 3:9f3e1a43f68f | 110 | |
higedura | 2:78c3d0598819 | 111 | for( int i=0;i<N;i++ ){ if( acc[i]<-2 ){ acc[i] = -2; } } |
higedura | 2:78c3d0598819 | 112 | for( int i=0;i<N;i++ ){ if( acc[i]>2 ){ acc[i] = 2; } } |
higedura | 2:78c3d0598819 | 113 | |
higedura | 2:78c3d0598819 | 114 | // Low pass filter for gyro |
higedura | 6:2f8a7136e326 | 115 | //for( int i=0;i<N;i++ ){ if( -0.5<gyro_deg[i] && gyro_deg[i]<0.5 ){ gyro_deg[i] = 0; } } |
higedura | 2:78c3d0598819 | 116 | |
higedura | 2:78c3d0598819 | 117 | for( int i=0;i<N;i++ ){ gyro_rad[i] = gyro_deg[i]*pi/180; } |
higedura | 2:78c3d0598819 | 118 | |
higedura | 2:78c3d0598819 | 119 | // Compass yaw |
higedura | 4:41289b278f12 | 120 | /*compass_rad = (double)mag[1]/mag[0]; |
higedura | 2:78c3d0598819 | 121 | compass_rad = atan(compass_rad); |
higedura | 2:78c3d0598819 | 122 | //compass_rad = compass_rad-compass_basis_rad; |
higedura | 2:78c3d0598819 | 123 | compass_deg = compass_rad*180/pi; |
higedura | 4:41289b278f12 | 124 | */ |
higedura | 2:78c3d0598819 | 125 | // LWMA (Observation) |
higedura | 6:2f8a7136e326 | 126 | p_zLWMA = LWMA(acc); |
higedura | 6:2f8a7136e326 | 127 | for( int i=0;i<N;i++ ){ zLWMA[i] = *p_zLWMA; p_zLWMA = p_zLWMA+1; } |
higedura | 2:78c3d0598819 | 128 | // LWMA angle |
higedura | 6:2f8a7136e326 | 129 | ang_acc[0] = asin(zLWMA[1])*180/pi; |
higedura | 4:41289b278f12 | 130 | ang_acc[1] = asin(zLWMA[0])*180/pi; |
higedura | 2:78c3d0598819 | 131 | |
higedura | 2:78c3d0598819 | 132 | // RK4 (Prediction) |
higedura | 6:2f8a7136e326 | 133 | p_RK4 = RK4(dt,ang_rad,gyro_rad); |
higedura | 6:2f8a7136e326 | 134 | for( int i=0;i<N;i++ ){ ang_rad[i] = *p_RK4; p_RK4 = p_RK4+1; } |
higedura | 2:78c3d0598819 | 135 | |
higedura | 2:78c3d0598819 | 136 | // EKF (Correction) |
higedura | 2:78c3d0598819 | 137 | EKF_predict(ang_rad,gyro_rad); |
higedura | 2:78c3d0598819 | 138 | correct_loop++; |
higedura | 4:41289b278f12 | 139 | if (correct_loop>=10){ |
higedura | 6:2f8a7136e326 | 140 | p_EKF = EKF_correct(ang_rad,gyro_rad,zLWMA); |
higedura | 6:2f8a7136e326 | 141 | for ( int i=0;i<N;i++ ){ ang_rad[i] = *p_EKF; p_EKF = p_EKF+1; } |
higedura | 2:78c3d0598819 | 142 | correct_loop = 0; |
higedura | 2:78c3d0598819 | 143 | } |
higedura | 2:78c3d0598819 | 144 | for( int i=0;i<N;i++ ){ ang_deg[i] = ang_rad[i]*180/pi; } |
higedura | 2:78c3d0598819 | 145 | |
higedura | 2:78c3d0598819 | 146 | //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]); |
higedura | 2:78c3d0598819 | 147 | //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]); |
higedura | 6:2f8a7136e326 | 148 | //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]); |
higedura | 6:2f8a7136e326 | 149 | 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]); |
higedura | 6:2f8a7136e326 | 150 | |
higedura | 4:41289b278f12 | 151 | wait(dt_wait); |
higedura | 2:78c3d0598819 | 152 | t += dt; |
higedura | 2:78c3d0598819 | 153 | |
higedura | 2:78c3d0598819 | 154 | } |
higedura | 2:78c3d0598819 | 155 | } |
higedura | 2:78c3d0598819 | 156 | |
higedura | 2:78c3d0598819 | 157 | |
higedura | 2:78c3d0598819 | 158 | double* EKF_predict( double y[N], double x[N] ){ |
higedura | 2:78c3d0598819 | 159 | // x = F * x; |
higedura | 2:78c3d0598819 | 160 | // P = F * P * F' + G * Q * G'; |
higedura | 2:78c3d0598819 | 161 | |
higedura | 6:2f8a7136e326 | 162 | //double Q[N][N] = { {0.1, 0, 0}, {0, 0.1, 0}, {0, 0, 0.1} }; |
higedura | 6:2f8a7136e326 | 163 | double Q[N][N] = { {0.00263, 0, 0}, {0, 0.00373, 0}, {0, 0, 0.00509} }; |
higedura | 2:78c3d0598819 | 164 | |
higedura | 2:78c3d0598819 | 165 | 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}}; |
higedura | 2:78c3d0598819 | 166 | 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}}; |
higedura | 2:78c3d0598819 | 167 | 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])}}; |
higedura | 2:78c3d0598819 | 168 | 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])}}; |
higedura | 2:78c3d0598819 | 169 | |
higedura | 2:78c3d0598819 | 170 | double FPF[N][N] = {0}; |
higedura | 2:78c3d0598819 | 171 | double GQG[N][N] = {0}; |
higedura | 2:78c3d0598819 | 172 | |
higedura | 2:78c3d0598819 | 173 | double FP[N][N] = {0}; |
higedura | 2:78c3d0598819 | 174 | double GQ[N][N] = {0}; |
higedura | 2:78c3d0598819 | 175 | |
higedura | 2:78c3d0598819 | 176 | for( int i=0;i<N;i++ ){ |
higedura | 2:78c3d0598819 | 177 | for( int j=0;j<N;j++ ){ |
higedura | 2:78c3d0598819 | 178 | for( int k=0;k<N;k++ ){ |
higedura | 2:78c3d0598819 | 179 | FP[i][j] += Fjac[i][k]*P[k][j]; |
higedura | 2:78c3d0598819 | 180 | GQ[i][j] += Gjac[i][k]*Q[k][j]; |
higedura | 2:78c3d0598819 | 181 | |
higedura | 2:78c3d0598819 | 182 | } |
higedura | 2:78c3d0598819 | 183 | } |
higedura | 2:78c3d0598819 | 184 | } |
higedura | 2:78c3d0598819 | 185 | |
higedura | 2:78c3d0598819 | 186 | for( int i=0;i<N;i++ ){ |
higedura | 2:78c3d0598819 | 187 | for( int j=0;j<N;j++ ){ |
higedura | 2:78c3d0598819 | 188 | for( int k=0;k<N;k++ ){ |
higedura | 2:78c3d0598819 | 189 | FPF[i][j] += FP[i][k]*Fjac_t[k][j]; |
higedura | 2:78c3d0598819 | 190 | GQG[i][j] += GQ[i][k]*Gjac_t[k][j]; |
higedura | 2:78c3d0598819 | 191 | } |
higedura | 2:78c3d0598819 | 192 | } |
higedura | 2:78c3d0598819 | 193 | } |
higedura | 2:78c3d0598819 | 194 | for( int i=0;i<N;i++ ){ |
higedura | 2:78c3d0598819 | 195 | for( int j=0;j<N;j++ ){ |
higedura | 2:78c3d0598819 | 196 | P[i][j] = FPF[i][j]+GQG[i][j]; |
higedura | 2:78c3d0598819 | 197 | } |
higedura | 2:78c3d0598819 | 198 | } |
higedura | 2:78c3d0598819 | 199 | |
higedura | 2:78c3d0598819 | 200 | return 0; |
higedura | 2:78c3d0598819 | 201 | |
higedura | 2:78c3d0598819 | 202 | } |
higedura | 2:78c3d0598819 | 203 | |
higedura | 2:78c3d0598819 | 204 | double* EKF_correct( double y[N], double x[N], double z[N] ){ |
higedura | 2:78c3d0598819 | 205 | // K = P * H' / ( H * P * H' + R ) |
higedura | 2:78c3d0598819 | 206 | // x = x + K * ( yobs(t) - H * x ) |
higedura | 2:78c3d0598819 | 207 | // P = P - K * H * P |
higedura | 2:78c3d0598819 | 208 | |
higedura | 6:2f8a7136e326 | 209 | //double R[N][N] = { {0.15, 0, 0}, {0, 0.15, 0}, {0, 0, 0.15} }; |
higedura | 6:2f8a7136e326 | 210 | double R[N][N] = { {0.00015, 0, 0}, {0, 0.00016, 0}, {0, 0, 0.00032} }; |
higedura | 2:78c3d0598819 | 211 | |
higedura | 2:78c3d0598819 | 212 | 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}}; |
higedura | 2:78c3d0598819 | 213 | 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}}; |
higedura | 2:78c3d0598819 | 214 | double K[N][N] = {0}; // Kalman gain |
higedura | 2:78c3d0598819 | 215 | double K_deno[N][N] = {0}; // Denominator of the kalman gain |
higedura | 2:78c3d0598819 | 216 | double det_K_deno_inv = 0; |
higedura | 2:78c3d0598819 | 217 | double K_deno_inv[N][N] = {0}; |
higedura | 2:78c3d0598819 | 218 | double HPH[N][N] = {0}; |
higedura | 2:78c3d0598819 | 219 | double HP[N][N] = {0}; |
higedura | 2:78c3d0598819 | 220 | double PH[N][N] = {0}; |
higedura | 2:78c3d0598819 | 221 | double KHP[N][N] = {0}; |
higedura | 2:78c3d0598819 | 222 | |
higedura | 2:78c3d0598819 | 223 | double Hx[N] = {0}; |
higedura | 2:78c3d0598819 | 224 | double z_Hx[N] = {0}; |
higedura | 2:78c3d0598819 | 225 | double Kz_Hx[N] = {0}; |
higedura | 2:78c3d0598819 | 226 | |
higedura | 2:78c3d0598819 | 227 | double* py = y; |
higedura | 2:78c3d0598819 | 228 | |
higedura | 2:78c3d0598819 | 229 | // Kalman gain |
higedura | 2:78c3d0598819 | 230 | for( int i=0;i<N;i++ ){ |
higedura | 2:78c3d0598819 | 231 | for( int j=0;j<N;j++ ){ |
higedura | 2:78c3d0598819 | 232 | for( int k=0;k<N;k++ ){ |
higedura | 2:78c3d0598819 | 233 | HP[i][j] += Hjac[i][k]*P[k][j]; |
higedura | 2:78c3d0598819 | 234 | PH[i][j] += P[i][k]*Hjac_t[k][j]; |
higedura | 2:78c3d0598819 | 235 | } |
higedura | 2:78c3d0598819 | 236 | } |
higedura | 2:78c3d0598819 | 237 | } |
higedura | 2:78c3d0598819 | 238 | for( int i=0;i<N;i++ ){ |
higedura | 2:78c3d0598819 | 239 | for( int j=0;j<N;j++ ){ |
higedura | 2:78c3d0598819 | 240 | for( int k=0;k<N;k++ ){ |
higedura | 2:78c3d0598819 | 241 | HPH[i][j] += HP[i][k]*Hjac_t[k][j]; |
higedura | 2:78c3d0598819 | 242 | } |
higedura | 2:78c3d0598819 | 243 | } |
higedura | 2:78c3d0598819 | 244 | } |
higedura | 2:78c3d0598819 | 245 | for( int i=0;i<N;i++ ){ |
higedura | 2:78c3d0598819 | 246 | for( int j=0;j<N;j++ ){ |
higedura | 2:78c3d0598819 | 247 | K_deno[i][j] = HPH[i][j]+R[i][j]; |
higedura | 2:78c3d0598819 | 248 | } |
higedura | 2:78c3d0598819 | 249 | } |
higedura | 3:9f3e1a43f68f | 250 | // Inverce matrix |
higedura | 2:78c3d0598819 | 251 | 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]; |
higedura | 2:78c3d0598819 | 252 | 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; |
higedura | 2:78c3d0598819 | 253 | 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; |
higedura | 2:78c3d0598819 | 254 | 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; |
higedura | 2:78c3d0598819 | 255 | 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; |
higedura | 2:78c3d0598819 | 256 | 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; |
higedura | 2:78c3d0598819 | 257 | 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; |
higedura | 2:78c3d0598819 | 258 | 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; |
higedura | 2:78c3d0598819 | 259 | 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; |
higedura | 2:78c3d0598819 | 260 | 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; |
higedura | 2:78c3d0598819 | 261 | |
higedura | 2:78c3d0598819 | 262 | for( int i=0;i<N;i++ ){ |
higedura | 2:78c3d0598819 | 263 | for( int j=0;j<N;j++ ){ |
higedura | 2:78c3d0598819 | 264 | for( int k=0;k<N;k++ ){ |
higedura | 2:78c3d0598819 | 265 | K[i][j] += PH[i][k]*K_deno_inv[k][j]; |
higedura | 2:78c3d0598819 | 266 | } |
higedura | 2:78c3d0598819 | 267 | } |
higedura | 2:78c3d0598819 | 268 | } |
higedura | 2:78c3d0598819 | 269 | |
higedura | 2:78c3d0598819 | 270 | // Filtering |
higedura | 2:78c3d0598819 | 271 | for( int i=0;i<N;i++ ){ |
higedura | 2:78c3d0598819 | 272 | for( int j=0;j<N;j++ ){ |
higedura | 2:78c3d0598819 | 273 | Hx[i] += Hjac[i][j]*y[j]; |
higedura | 2:78c3d0598819 | 274 | } |
higedura | 2:78c3d0598819 | 275 | } |
higedura | 2:78c3d0598819 | 276 | for( int i=0;i<N;i++ ){ |
higedura | 2:78c3d0598819 | 277 | z_Hx[i] = z[i]-Hx[i]; |
higedura | 2:78c3d0598819 | 278 | } |
higedura | 2:78c3d0598819 | 279 | for( int i=0;i<N;i++ ){ |
higedura | 2:78c3d0598819 | 280 | for( int j=0;j<N;j++ ){ |
higedura | 2:78c3d0598819 | 281 | Kz_Hx[i] += K[i][j]*z_Hx[j]; |
higedura | 2:78c3d0598819 | 282 | } |
higedura | 2:78c3d0598819 | 283 | } |
higedura | 2:78c3d0598819 | 284 | for( int i=0;i<N;i++ ){ |
higedura | 2:78c3d0598819 | 285 | y[i] = y[i]+Kz_Hx[i]; |
higedura | 2:78c3d0598819 | 286 | } |
higedura | 2:78c3d0598819 | 287 | |
higedura | 2:78c3d0598819 | 288 | // Covarience |
higedura | 2:78c3d0598819 | 289 | for( int i=0;i<N;i++ ){ |
higedura | 2:78c3d0598819 | 290 | for( int j=0;j<N;j++ ){ |
higedura | 2:78c3d0598819 | 291 | for( int k=0;k<N;k++ ){ |
higedura | 2:78c3d0598819 | 292 | KHP[i][j] += K[i][k]*HP[k][j]; |
higedura | 2:78c3d0598819 | 293 | } |
higedura | 2:78c3d0598819 | 294 | } |
higedura | 2:78c3d0598819 | 295 | } |
higedura | 2:78c3d0598819 | 296 | for( int i=0;i<N;i++ ){ |
higedura | 2:78c3d0598819 | 297 | for( int j=0;j<N;j++ ){ |
higedura | 2:78c3d0598819 | 298 | P[i][j] = P[i][j]-KHP[i][j]; |
higedura | 2:78c3d0598819 | 299 | } |
higedura | 2:78c3d0598819 | 300 | } |
higedura | 2:78c3d0598819 | 301 | |
higedura | 2:78c3d0598819 | 302 | return py; |
higedura | 2:78c3d0598819 | 303 | |
higedura | 2:78c3d0598819 | 304 | } |
higedura | 2:78c3d0598819 | 305 | |
higedura | 2:78c3d0598819 | 306 | double* LWMA( double z[N] ){ |
higedura | 2:78c3d0598819 | 307 | |
higedura | 2:78c3d0598819 | 308 | double zLWMA[N] = {0}; |
higedura | 2:78c3d0598819 | 309 | double zLWMA_num[N] = {0}; |
higedura | 2:78c3d0598819 | 310 | double zLWMA_den = 0; |
higedura | 2:78c3d0598819 | 311 | |
higedura | 6:2f8a7136e326 | 312 | double* p_zLWMA = zLWMA; |
higedura | 2:78c3d0598819 | 313 | |
higedura | 2:78c3d0598819 | 314 | for( int i=1;i<N_LWMA;i++ ){ |
higedura | 2:78c3d0598819 | 315 | for( int j=0;j<N;j++ ){ |
higedura | 2:78c3d0598819 | 316 | z_buf[j][N_LWMA-i] = z_buf[j][N_LWMA-i-1]; |
higedura | 2:78c3d0598819 | 317 | } |
higedura | 2:78c3d0598819 | 318 | } |
higedura | 2:78c3d0598819 | 319 | for( int i=0;i<N;i++ ){ |
higedura | 2:78c3d0598819 | 320 | z_buf[i][0] = z[i]; |
higedura | 2:78c3d0598819 | 321 | } |
higedura | 2:78c3d0598819 | 322 | |
higedura | 2:78c3d0598819 | 323 | for( int i=0;i<N_LWMA;i++ ){ |
higedura | 2:78c3d0598819 | 324 | for( int j=0;j<N;j++ ){ |
higedura | 2:78c3d0598819 | 325 | zLWMA_num[j] += (N_LWMA-i)*z_buf[j][i]; |
higedura | 2:78c3d0598819 | 326 | } |
higedura | 2:78c3d0598819 | 327 | zLWMA_den += N_LWMA-i; |
higedura | 2:78c3d0598819 | 328 | } |
higedura | 2:78c3d0598819 | 329 | for( int i=0;i<N;i++ ){ |
higedura | 2:78c3d0598819 | 330 | zLWMA[i] = zLWMA_num[i]/zLWMA_den; |
higedura | 2:78c3d0598819 | 331 | } |
higedura | 2:78c3d0598819 | 332 | |
higedura | 6:2f8a7136e326 | 333 | return p_zLWMA; |
higedura | 2:78c3d0598819 | 334 | |
higedura | 2:78c3d0598819 | 335 | } |
higedura | 2:78c3d0598819 | 336 | |
higedura | 2:78c3d0598819 | 337 | double* RK4( double dt, double y[N], double x[N] ){ |
higedura | 2:78c3d0598819 | 338 | |
higedura | 2:78c3d0598819 | 339 | double yBuf[N] = {0}; |
higedura | 2:78c3d0598819 | 340 | double k[N][4] = {0}; |
higedura | 2:78c3d0598819 | 341 | |
higedura | 2:78c3d0598819 | 342 | double* p_y = y; |
higedura | 2:78c3d0598819 | 343 | |
higedura | 2:78c3d0598819 | 344 | double* pk1; |
higedura | 2:78c3d0598819 | 345 | double* pk2; |
higedura | 2:78c3d0598819 | 346 | double* pk3; |
higedura | 2:78c3d0598819 | 347 | double* pk4; |
higedura | 2:78c3d0598819 | 348 | |
higedura | 2:78c3d0598819 | 349 | for( int i=0;i<N;i++){ yBuf[i] = y[i]; } |
higedura | 2:78c3d0598819 | 350 | pk1 = func (yBuf,x); |
higedura | 2:78c3d0598819 | 351 | for( int i=0;i<N;i++ ){ k[i][0] = *pk1; pk1 = pk1+1; } |
higedura | 2:78c3d0598819 | 352 | |
higedura | 2:78c3d0598819 | 353 | for( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][1]; } |
higedura | 2:78c3d0598819 | 354 | pk2 = func (yBuf,x); |
higedura | 2:78c3d0598819 | 355 | for( int i=0;i<N;i++ ){ k[i][1] = *pk2; pk2 = pk2+1; } |
higedura | 2:78c3d0598819 | 356 | |
higedura | 2:78c3d0598819 | 357 | for( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][2]; } |
higedura | 2:78c3d0598819 | 358 | pk3 = func (yBuf,x); |
higedura | 2:78c3d0598819 | 359 | for( int i=0;i<N;i++ ){ k[i][2] = *pk3; pk3 = pk3+1; } |
higedura | 2:78c3d0598819 | 360 | |
higedura | 2:78c3d0598819 | 361 | for( int i=0;i<N;i++){ yBuf[i] = y[i]+dt*k[i][3]; } |
higedura | 2:78c3d0598819 | 362 | pk4 = func (yBuf,x); |
higedura | 2:78c3d0598819 | 363 | for( int i=0;i<N;i++ ){ k[i][3] = *pk4; pk4 = pk4+1; } |
higedura | 2:78c3d0598819 | 364 | |
higedura | 2:78c3d0598819 | 365 | 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; } |
higedura | 2:78c3d0598819 | 366 | |
higedura | 2:78c3d0598819 | 367 | return p_y; |
higedura | 2:78c3d0598819 | 368 | |
higedura | 2:78c3d0598819 | 369 | } |
higedura | 2:78c3d0598819 | 370 | |
higedura | 2:78c3d0598819 | 371 | double* func( double y[N], double x[N] ){ |
higedura | 2:78c3d0598819 | 372 | |
higedura | 2:78c3d0598819 | 373 | double f[N] = {0}; |
higedura | 2:78c3d0598819 | 374 | double* p_f = f; |
higedura | 2:78c3d0598819 | 375 | |
higedura | 2:78c3d0598819 | 376 | f[0] = x[0]+x[1]*sin(y[0])*tan(y[1])+x[2]*cos(y[0])*tan(y[1]); |
higedura | 2:78c3d0598819 | 377 | f[1] = x[1]*cos(y[0])-x[2]*sin(y[0]); |
higedura | 2:78c3d0598819 | 378 | f[2] = x[1]*sin(y[0])/cos(y[1])+x[2]*cos(y[0])/cos(y[1]); |
higedura | 2:78c3d0598819 | 379 | |
higedura | 2:78c3d0598819 | 380 | return p_f; |
higedura | 2:78c3d0598819 | 381 | |
higedura | 2:78c3d0598819 | 382 | } |
higedura | 6:2f8a7136e326 | 383 | |
higedura | 6:2f8a7136e326 | 384 | double* calib(){ |
higedura | 6:2f8a7136e326 | 385 | |
higedura | 6:2f8a7136e326 | 386 | int calib_loop = 1000; |
higedura | 6:2f8a7136e326 | 387 | |
higedura | 6:2f8a7136e326 | 388 | int bit_acc[N] = {0}; // Buffer of the accelerometer |
higedura | 6:2f8a7136e326 | 389 | int get_mag[N] = {0}; // Buffer of the compass |
higedura | 6:2f8a7136e326 | 390 | |
higedura | 6:2f8a7136e326 | 391 | double calib_acc[N] = {0}; |
higedura | 6:2f8a7136e326 | 392 | double calib_gyro_buf[N] = {0}; |
higedura | 6:2f8a7136e326 | 393 | double calib_gyro[N] = {0}; |
higedura | 6:2f8a7136e326 | 394 | double compass_basis_buf[N] = {0}; |
higedura | 6:2f8a7136e326 | 395 | double compass_basis_rad = 0; |
higedura | 6:2f8a7136e326 | 396 | double calib_result[7] = {0}; |
higedura | 6:2f8a7136e326 | 397 | |
higedura | 6:2f8a7136e326 | 398 | double* p_calib_result = calib_result; |
higedura | 6:2f8a7136e326 | 399 | |
higedura | 10:3cfc137e23e8 | 400 | pc.printf("\n\r\n\rDon't touch... Calibrating now!!\n\r"); |
higedura | 6:2f8a7136e326 | 401 | led1 = 1; |
higedura | 6:2f8a7136e326 | 402 | |
higedura | 6:2f8a7136e326 | 403 | for( int i=0;i<calib_loop;i++ ){ |
higedura | 6:2f8a7136e326 | 404 | |
higedura | 6:2f8a7136e326 | 405 | accelerometer.getOutput(bit_acc); |
higedura | 6:2f8a7136e326 | 406 | compass.readData(get_mag); |
higedura | 6:2f8a7136e326 | 407 | |
higedura | 6:2f8a7136e326 | 408 | calib_gyro_buf[0] = gyro.getGyroX(); |
higedura | 6:2f8a7136e326 | 409 | calib_gyro_buf[1] = gyro.getGyroY(); |
higedura | 6:2f8a7136e326 | 410 | calib_gyro_buf[2] = gyro.getGyroZ(); |
higedura | 6:2f8a7136e326 | 411 | |
higedura | 6:2f8a7136e326 | 412 | for( int j=0;j<N;j++ ){ |
higedura | 6:2f8a7136e326 | 413 | calib_acc[j] += (int16_t)bit_acc[j]; |
higedura | 6:2f8a7136e326 | 414 | calib_gyro[j] += calib_gyro_buf[j]; |
higedura | 6:2f8a7136e326 | 415 | compass_basis_buf[j] += (int16_t)get_mag[j]; |
higedura | 6:2f8a7136e326 | 416 | } |
higedura | 6:2f8a7136e326 | 417 | |
higedura | 6:2f8a7136e326 | 418 | if( i>calib_loop*3/4 ){ |
higedura | 6:2f8a7136e326 | 419 | led4 = 1; |
higedura | 6:2f8a7136e326 | 420 | }else if( i>calib_loop/2 ){ |
higedura | 6:2f8a7136e326 | 421 | led3 = 1; |
higedura | 6:2f8a7136e326 | 422 | }else if( i>calib_loop/4 ){ |
higedura | 6:2f8a7136e326 | 423 | led2 = 1; |
higedura | 6:2f8a7136e326 | 424 | } |
higedura | 6:2f8a7136e326 | 425 | |
higedura | 6:2f8a7136e326 | 426 | } |
higedura | 6:2f8a7136e326 | 427 | |
higedura | 6:2f8a7136e326 | 428 | for( int i=0;i<N;i++ ){ |
higedura | 6:2f8a7136e326 | 429 | calib_acc[i] = calib_acc[i]/calib_loop; |
higedura | 6:2f8a7136e326 | 430 | calib_gyro[i] = calib_gyro[i]/calib_loop; |
higedura | 6:2f8a7136e326 | 431 | compass_basis_buf[i] = compass_basis_buf[i]/calib_loop; |
higedura | 6:2f8a7136e326 | 432 | } |
higedura | 6:2f8a7136e326 | 433 | |
higedura | 6:2f8a7136e326 | 434 | compass_basis_rad = compass_basis_buf[1]/compass_basis_buf[0]; |
higedura | 6:2f8a7136e326 | 435 | compass_basis_rad = atan(compass_basis_rad); |
higedura | 6:2f8a7136e326 | 436 | led1 = 0; led2 = 0; led3 = 0; led4 = 0; |
higedura | 6:2f8a7136e326 | 437 | |
higedura | 8:56fb656dcef6 | 438 | pc.printf(" accX accY accZ gyroX gyroY gyroZ yaw_basis\n\r"); |
higedura | 8:56fb656dcef6 | 439 | 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); |
higedura | 6:2f8a7136e326 | 440 | |
higedura | 6:2f8a7136e326 | 441 | for( int i=0;i<3;i++ ){ calib_result[i] = calib_acc[i]; } |
higedura | 6:2f8a7136e326 | 442 | for( int i=3;i<6;i++ ){ calib_result[i] = calib_gyro[i-3]; } |
higedura | 6:2f8a7136e326 | 443 | calib_result[6] = compass_basis_rad; |
higedura | 6:2f8a7136e326 | 444 | |
higedura | 6:2f8a7136e326 | 445 | for( int i=0;i<3;i++ ){ |
higedura | 6:2f8a7136e326 | 446 | wait(0.5); |
higedura | 6:2f8a7136e326 | 447 | led1 = 1; led2 = 1; led3 = 1; led4 = 1; |
higedura | 6:2f8a7136e326 | 448 | wait(0.5); |
higedura | 6:2f8a7136e326 | 449 | led1 = 0; led2 = 0; led3 = 0; led4 = 0; |
higedura | 6:2f8a7136e326 | 450 | } |
higedura | 6:2f8a7136e326 | 451 | |
higedura | 6:2f8a7136e326 | 452 | return p_calib_result; |
higedura | 6:2f8a7136e326 | 453 | |
higedura | 6:2f8a7136e326 | 454 | } |
higedura | 6:2f8a7136e326 | 455 | |
higedura | 6:2f8a7136e326 | 456 | int preparing_acc(){ |
higedura | 6:2f8a7136e326 | 457 | |
higedura | 6:2f8a7136e326 | 458 | // These are here to test whether any of the initialization fails. It will print the failure. |
higedura | 6:2f8a7136e326 | 459 | if (accelerometer.setPowerControl(0x00)){ |
higedura | 6:2f8a7136e326 | 460 | pc.printf("didn't intitialize power control\n\r"); |
higedura | 6:2f8a7136e326 | 461 | return 0; |
higedura | 6:2f8a7136e326 | 462 | } |
higedura | 6:2f8a7136e326 | 463 | // Full resolution, +/-16g, 4mg/LSB. |
higedura | 6:2f8a7136e326 | 464 | wait(.001); |
higedura | 6:2f8a7136e326 | 465 | |
higedura | 6:2f8a7136e326 | 466 | if(accelerometer.setDataFormatControl(0x0B)){ |
higedura | 6:2f8a7136e326 | 467 | pc.printf("didn't set data format\n\r"); |
higedura | 6:2f8a7136e326 | 468 | return 0; |
higedura | 6:2f8a7136e326 | 469 | } |
higedura | 6:2f8a7136e326 | 470 | wait(.001); |
higedura | 6:2f8a7136e326 | 471 | |
higedura | 6:2f8a7136e326 | 472 | // 3.2kHz data rate. |
higedura | 6:2f8a7136e326 | 473 | if(accelerometer.setDataRate(ADXL345_3200HZ)){ |
higedura | 6:2f8a7136e326 | 474 | pc.printf("didn't set data rate\n\r"); |
higedura | 6:2f8a7136e326 | 475 | return 0; |
higedura | 6:2f8a7136e326 | 476 | } |
higedura | 6:2f8a7136e326 | 477 | wait(.001); |
higedura | 6:2f8a7136e326 | 478 | |
higedura | 6:2f8a7136e326 | 479 | if(accelerometer.setPowerControl(MeasurementMode)) { |
higedura | 6:2f8a7136e326 | 480 | pc.printf("didn't set the power control to measurement\n\r"); |
higedura | 6:2f8a7136e326 | 481 | return 0; |
higedura | 6:2f8a7136e326 | 482 | } |
higedura | 6:2f8a7136e326 | 483 | |
higedura | 6:2f8a7136e326 | 484 | return 0; |
higedura | 6:2f8a7136e326 | 485 | |
higedura | 6:2f8a7136e326 | 486 | } |