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