han back / Mbed OS CLEO_UART_MPU9250_PC

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

main.cpp@0:82ad978ba101, 2017-09-28 (annotated)

Committer:
SMART_CLEO
Date:
Thu Sep 28 03:36:54 2017 +0000
Revision:
0:82ad978ba101
Child:
1:bb8075327fa6
SMART_CLEO

Who changed what in which revision?

UserRevisionLine numberNew contents of line
SMART_CLEO 0:82ad978ba101 1 #include "mbed.h"
SMART_CLEO 0:82ad978ba101 2 #include "MPU9250.h"
SMART_CLEO 0:82ad978ba101 3 #include "TextLCD.h"
SMART_CLEO 0:82ad978ba101 4
SMART_CLEO 0:82ad978ba101 5 struct UART_buf
SMART_CLEO 0:82ad978ba101 6 {
SMART_CLEO 0:82ad978ba101 7 uint8_t STA;
SMART_CLEO 0:82ad978ba101 8 uint8_t MODE;
SMART_CLEO 0:82ad978ba101 9 uint8_t CMD;
SMART_CLEO 0:82ad978ba101 10 uint8_t LEN;
SMART_CLEO 0:82ad978ba101 11 uint8_t DATA[32];
SMART_CLEO 0:82ad978ba101 12 uint8_t END;
SMART_CLEO 0:82ad978ba101 13
SMART_CLEO 0:82ad978ba101 14 };
SMART_CLEO 0:82ad978ba101 15
SMART_CLEO 0:82ad978ba101 16 union Data_DB{
SMART_CLEO 0:82ad978ba101 17 int16_t data16;
SMART_CLEO 0:82ad978ba101 18 uint8_t data8[2];
SMART_CLEO 0:82ad978ba101 19 }Data_Tr;
SMART_CLEO 0:82ad978ba101 20
SMART_CLEO 0:82ad978ba101 21 MPU9250 mpu9250;
SMART_CLEO 0:82ad978ba101 22
SMART_CLEO 0:82ad978ba101 23 Serial SerialUART(PA_2, PA_3); // tx, rx
SMART_CLEO 0:82ad978ba101 24
SMART_CLEO 0:82ad978ba101 25 // rs, rw, e, d0-d3
SMART_CLEO 0:82ad978ba101 26 TextLCD lcd(PB_12, PB_13, PB_14, PB_15, PA_9, PA_10, PA_11);
SMART_CLEO 0:82ad978ba101 27
SMART_CLEO 0:82ad978ba101 28 #define N 3
SMART_CLEO 0:82ad978ba101 29 #define N_LWMA 10
SMART_CLEO 0:82ad978ba101 30
SMART_CLEO 0:82ad978ba101 31 uint8_t Buffer[37];
SMART_CLEO 0:82ad978ba101 32 volatile uint8_t Sensor_flag = 0;
SMART_CLEO 0:82ad978ba101 33
SMART_CLEO 0:82ad978ba101 34 UART_buf RX_BUF;
SMART_CLEO 0:82ad978ba101 35
SMART_CLEO 0:82ad978ba101 36 float sum = 0, yaw_an = 0;
SMART_CLEO 0:82ad978ba101 37 uint32_t sumCount = 0;
SMART_CLEO 0:82ad978ba101 38
SMART_CLEO 0:82ad978ba101 39 Timer t;
SMART_CLEO 0:82ad978ba101 40
SMART_CLEO 0:82ad978ba101 41 double Accel[N], Gyro[N], Mag[N], angle[N] = {0, 0, 0};
SMART_CLEO 0:82ad978ba101 42 volatile uint8_t filter_flag = 0, filter_pre = 5;
SMART_CLEO 0:82ad978ba101 43
SMART_CLEO 0:82ad978ba101 44 double z_buf[N][N_LWMA] = {0}; // For LWMA
SMART_CLEO 0:82ad978ba101 45
SMART_CLEO 0:82ad978ba101 46 double P[N][N] = {{1,0,0},{0,1,0},{0,0,1}}; // For EKF
SMART_CLEO 0:82ad978ba101 47
SMART_CLEO 0:82ad978ba101 48 double* EKF_predict( double y[N], double x[N] );
SMART_CLEO 0:82ad978ba101 49 double* EKF_correct( double y[N], double x[N], double z[N] );
SMART_CLEO 0:82ad978ba101 50 double* LWMA( double z[N] );
SMART_CLEO 0:82ad978ba101 51 double* RK4( double dt, double y[N], double x[N] );
SMART_CLEO 0:82ad978ba101 52 double* func( double y[N], double x[N] );
SMART_CLEO 0:82ad978ba101 53 void SerialUARTRX_ISR(void);
SMART_CLEO 0:82ad978ba101 54 void Timer_setting(uint8_t cmd, uint8_t value);
SMART_CLEO 0:82ad978ba101 55 void Sensor_Read(void);
SMART_CLEO 0:82ad978ba101 56
SMART_CLEO 0:82ad978ba101 57 int main()
SMART_CLEO 0:82ad978ba101 58 {
SMART_CLEO 0:82ad978ba101 59 int16_t az_tmp = 0;
SMART_CLEO 0:82ad978ba101 60
SMART_CLEO 0:82ad978ba101 61 SerialUART.baud(115200);
SMART_CLEO 0:82ad978ba101 62
SMART_CLEO 0:82ad978ba101 63 i2c.frequency(400000); // use fast (400 kHz) I2C
SMART_CLEO 0:82ad978ba101 64
SMART_CLEO 0:82ad978ba101 65 int correct_loop = 0;
SMART_CLEO 0:82ad978ba101 66
SMART_CLEO 0:82ad978ba101 67 double ang_acc[2] = {0,0};
SMART_CLEO 0:82ad978ba101 68 double ang_deg[N] = {0,0,0};
SMART_CLEO 0:82ad978ba101 69 double gyro_rad[N] = {0,0,0};
SMART_CLEO 0:82ad978ba101 70 double ang_rad[N] = {0,0,0};
SMART_CLEO 0:82ad978ba101 71 double zLWMA[N] = {0,0,0};
SMART_CLEO 0:82ad978ba101 72
SMART_CLEO 0:82ad978ba101 73 double* p_RK4;
SMART_CLEO 0:82ad978ba101 74 double* p_EKF;
SMART_CLEO 0:82ad978ba101 75 double* p_zLWMA;
SMART_CLEO 0:82ad978ba101 76
SMART_CLEO 0:82ad978ba101 77 for( int i=0;i<N_LWMA;i++ ){ z_buf[2][i] = 1; }
SMART_CLEO 0:82ad978ba101 78
SMART_CLEO 0:82ad978ba101 79 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
SMART_CLEO 0:82ad978ba101 80
SMART_CLEO 0:82ad978ba101 81 if (whoami == 0x71) // WHO_AM_I should always be 0x68
SMART_CLEO 0:82ad978ba101 82 {
SMART_CLEO 0:82ad978ba101 83 lcd.printf("MPU9250 is 0x%x\n",whoami);
SMART_CLEO 0:82ad978ba101 84 lcd.printf(" Connected ");
SMART_CLEO 0:82ad978ba101 85
SMART_CLEO 0:82ad978ba101 86 wait(1);
SMART_CLEO 0:82ad978ba101 87
SMART_CLEO 0:82ad978ba101 88 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
SMART_CLEO 0:82ad978ba101 89 mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
SMART_CLEO 0:82ad978ba101 90 /*SerialUART.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);
SMART_CLEO 0:82ad978ba101 91 SerialUART.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);
SMART_CLEO 0:82ad978ba101 92 SerialUART.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);
SMART_CLEO 0:82ad978ba101 93 SerialUART.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);
SMART_CLEO 0:82ad978ba101 94 SerialUART.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);
SMART_CLEO 0:82ad978ba101 95 SerialUART.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]); */
SMART_CLEO 0:82ad978ba101 96 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
SMART_CLEO 0:82ad978ba101 97 /*SerialUART.printf("x gyro bias = %f\n\r", gyroBias[0]);
SMART_CLEO 0:82ad978ba101 98 SerialUART.printf("y gyro bias = %f\n\r", gyroBias[1]);
SMART_CLEO 0:82ad978ba101 99 SerialUART.printf("z gyro bias = %f\n\r", gyroBias[2]);
SMART_CLEO 0:82ad978ba101 100 SerialUART.printf("x accel bias = %f\n\r", accelBias[0]);
SMART_CLEO 0:82ad978ba101 101 SerialUART.printf("y accel bias = %f\n\r", accelBias[1]);
SMART_CLEO 0:82ad978ba101 102 SerialUART.printf("z accel bias = %f\n\r", accelBias[2]);*/
SMART_CLEO 0:82ad978ba101 103 wait(2);
SMART_CLEO 0:82ad978ba101 104 mpu9250.initMPU9250();
SMART_CLEO 0:82ad978ba101 105 //SerialUART.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
SMART_CLEO 0:82ad978ba101 106 mpu9250.initAK8963(magCalibration);
SMART_CLEO 0:82ad978ba101 107 /*SerialUART.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
SMART_CLEO 0:82ad978ba101 108 SerialUART.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
SMART_CLEO 0:82ad978ba101 109 pSerialUARTc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
SMART_CLEO 0:82ad978ba101 110 if(Mscale == 0) SerialUART.printf("Magnetometer resolution = 14 bits\n\r");
SMART_CLEO 0:82ad978ba101 111 if(Mscale == 1) SerialUART.printf("Magnetometer resolution = 16 bits\n\r");
SMART_CLEO 0:82ad978ba101 112 if(Mmode == 2) SerialUART.printf("Magnetometer ODR = 8 Hz\n\r");
SMART_CLEO 0:82ad978ba101 113 if(Mmode == 6) SerialUART.printf("Magnetometer ODR = 100 Hz\n\r");*/
SMART_CLEO 0:82ad978ba101 114 wait(1);
SMART_CLEO 0:82ad978ba101 115 }
SMART_CLEO 0:82ad978ba101 116 else
SMART_CLEO 0:82ad978ba101 117 {
SMART_CLEO 0:82ad978ba101 118 //SerialUART.printf("Could not connect to MPU9250: \n\r");
SMART_CLEO 0:82ad978ba101 119 //SerialUART.printf("%#x \n", whoami);
SMART_CLEO 0:82ad978ba101 120
SMART_CLEO 0:82ad978ba101 121 lcd.printf("MPU9250 is 0x%x\n",whoami);
SMART_CLEO 0:82ad978ba101 122 lcd.printf(" No connection ");
SMART_CLEO 0:82ad978ba101 123
SMART_CLEO 0:82ad978ba101 124 while(1) ; // Loop forever if communication doesn't happen
SMART_CLEO 0:82ad978ba101 125 }
SMART_CLEO 0:82ad978ba101 126
SMART_CLEO 0:82ad978ba101 127 t.start();
SMART_CLEO 0:82ad978ba101 128
SMART_CLEO 0:82ad978ba101 129 mpu9250.getAres(); // Get accelerometer sensitivity
SMART_CLEO 0:82ad978ba101 130 mpu9250.getGres(); // Get gyro sensitivity
SMART_CLEO 0:82ad978ba101 131 mpu9250.getMres(); // Get magnetometer sensitivity
SMART_CLEO 0:82ad978ba101 132 /* pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
SMART_CLEO 0:82ad978ba101 133 pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
SMART_CLEO 0:82ad978ba101 134 pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);*/
SMART_CLEO 0:82ad978ba101 135 magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
SMART_CLEO 0:82ad978ba101 136 magbias[1] = +120.; // User environmental x-axis correction in milliGauss
SMART_CLEO 0:82ad978ba101 137 magbias[2] = +125.; // User environmental x-axis correction in milliGauss
SMART_CLEO 0:82ad978ba101 138
SMART_CLEO 0:82ad978ba101 139 SerialUART.attach(&SerialUARTRX_ISR);
SMART_CLEO 0:82ad978ba101 140
SMART_CLEO 0:82ad978ba101 141 //Timer_setting(0x06, 200);
SMART_CLEO 0:82ad978ba101 142 //Sensor_Timer.attach(&Sensor_Read, 0.005);
SMART_CLEO 0:82ad978ba101 143
SMART_CLEO 0:82ad978ba101 144 while(1) {
SMART_CLEO 0:82ad978ba101 145
SMART_CLEO 0:82ad978ba101 146 // If intPin goes high, all data registers have new data
SMART_CLEO 0:82ad978ba101 147 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
SMART_CLEO 0:82ad978ba101 148
SMART_CLEO 0:82ad978ba101 149 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
SMART_CLEO 0:82ad978ba101 150 // Now we'll calculate the accleration value into actual g's
SMART_CLEO 0:82ad978ba101 151 for(int i=0; i<3; i++)
SMART_CLEO 0:82ad978ba101 152 {
SMART_CLEO 0:82ad978ba101 153 Accel[i] = (float)accelCount[i]*aRes - accelBias[i]; // get actual g value, this depends on scale being set
SMART_CLEO 0:82ad978ba101 154 }
SMART_CLEO 0:82ad978ba101 155 /*
SMART_CLEO 0:82ad978ba101 156 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
SMART_CLEO 0:82ad978ba101 157 ay = (float)accelCount[1]*aRes - accelBias[1];
SMART_CLEO 0:82ad978ba101 158 az = (float)accelCount[2]*aRes - accelBias[2]; */
SMART_CLEO 0:82ad978ba101 159
SMART_CLEO 0:82ad978ba101 160 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
SMART_CLEO 0:82ad978ba101 161 // Calculate the gyro value into actual degrees per second
SMART_CLEO 0:82ad978ba101 162 for(int i=0; i<3; i++)
SMART_CLEO 0:82ad978ba101 163 {
SMART_CLEO 0:82ad978ba101 164 Gyro[i] = (float)gyroCount[i]*gRes - gyroBias[i]; // get actual gyro value, this depends on scale being set
SMART_CLEO 0:82ad978ba101 165 }
SMART_CLEO 0:82ad978ba101 166 /*
SMART_CLEO 0:82ad978ba101 167 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
SMART_CLEO 0:82ad978ba101 168 gy = (float)gyroCount[1]*gRes - gyroBias[1];
SMART_CLEO 0:82ad978ba101 169 gz = (float)gyroCount[2]*gRes - gyroBias[2]; */
SMART_CLEO 0:82ad978ba101 170
SMART_CLEO 0:82ad978ba101 171 mpu9250.readMagData(magCount); // Read the x/y/z adc values
SMART_CLEO 0:82ad978ba101 172 // Calculate the magnetometer values in milliGauss
SMART_CLEO 0:82ad978ba101 173 // Include factory calibration per data sheet and user environmental corrections
SMART_CLEO 0:82ad978ba101 174 for(int i=0; i<3; i++)
SMART_CLEO 0:82ad978ba101 175 {
SMART_CLEO 0:82ad978ba101 176 Mag[i] = (float)magCount[i]*mRes*magCalibration[i] - magbias[i]; // get actual magnetometer value, this depends on scale being set
SMART_CLEO 0:82ad978ba101 177 }
SMART_CLEO 0:82ad978ba101 178 /*
SMART_CLEO 0:82ad978ba101 179 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
SMART_CLEO 0:82ad978ba101 180 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
SMART_CLEO 0:82ad978ba101 181 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2]; */
SMART_CLEO 0:82ad978ba101 182 }
SMART_CLEO 0:82ad978ba101 183
SMART_CLEO 0:82ad978ba101 184 Now = t.read_us();
SMART_CLEO 0:82ad978ba101 185 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
SMART_CLEO 0:82ad978ba101 186 lastUpdate = Now;
SMART_CLEO 0:82ad978ba101 187 az_tmp = Gyro[2];
SMART_CLEO 0:82ad978ba101 188 yaw_an += ((float)az_tmp * deltat);
SMART_CLEO 0:82ad978ba101 189 sum += deltat;
SMART_CLEO 0:82ad978ba101 190 sumCount++;
SMART_CLEO 0:82ad978ba101 191
SMART_CLEO 0:82ad978ba101 192 // if(lastUpdate - firstUpdate > 10000000.0f) {
SMART_CLEO 0:82ad978ba101 193 // beta = 0.04; // decrease filter gain after stabilized
SMART_CLEO 0:82ad978ba101 194 // zeta = 0.015; // increasey bias drift gain after stabilized
SMART_CLEO 0:82ad978ba101 195 // }
SMART_CLEO 0:82ad978ba101 196 if(filter_flag != 1)
SMART_CLEO 0:82ad978ba101 197 {
SMART_CLEO 0:82ad978ba101 198 // Pass gyro rate as rad/s
SMART_CLEO 0:82ad978ba101 199 mpu9250.MadgwickQuaternionUpdate(Accel[0], Accel[1], Accel[2], Gyro[0]*PI/180.0f, Gyro[1]*PI/180.0f, Gyro[2]*PI/180.0f, Mag[1], Mag[0], Mag[2]);
SMART_CLEO 0:82ad978ba101 200 mpu9250.MahonyQuaternionUpdate(Accel[0], Accel[1], Accel[2], Gyro[0]*PI/180.0f, Gyro[1]*PI/180.0f, Gyro[2]*PI/180.0f, Mag[1], Mag[0], Mag[2]);
SMART_CLEO 0:82ad978ba101 201 }
SMART_CLEO 0:82ad978ba101 202 if(filter_flag == 1)
SMART_CLEO 0:82ad978ba101 203 {
SMART_CLEO 0:82ad978ba101 204
SMART_CLEO 0:82ad978ba101 205 for( int i=0;i<N;i++ ){ gyro_rad[i] = Gyro[i]*PI/180; }
SMART_CLEO 0:82ad978ba101 206
SMART_CLEO 0:82ad978ba101 207 // Compass yaw
SMART_CLEO 0:82ad978ba101 208 /*compass_rad = (double)mag[1]/mag[0];
SMART_CLEO 0:82ad978ba101 209 compass_rad = atan(compass_rad);
SMART_CLEO 0:82ad978ba101 210 //compass_rad = compass_rad-compass_basis_rad;
SMART_CLEO 0:82ad978ba101 211 compass_deg = compass_rad*180/pi;
SMART_CLEO 0:82ad978ba101 212 */
SMART_CLEO 0:82ad978ba101 213 // LWMA (Observation)
SMART_CLEO 0:82ad978ba101 214 p_zLWMA = LWMA(Accel);
SMART_CLEO 0:82ad978ba101 215 for( int i=0;i<N;i++ )
SMART_CLEO 0:82ad978ba101 216 {
SMART_CLEO 0:82ad978ba101 217 zLWMA[i] = *p_zLWMA;
SMART_CLEO 0:82ad978ba101 218 p_zLWMA++;
SMART_CLEO 0:82ad978ba101 219 }
SMART_CLEO 0:82ad978ba101 220 // LWMA angle
SMART_CLEO 0:82ad978ba101 221 ang_acc[0] = asin(zLWMA[1])*180/PI;
SMART_CLEO 0:82ad978ba101 222 ang_acc[1] = asin(zLWMA[0])*180/PI;
SMART_CLEO 0:82ad978ba101 223
SMART_CLEO 0:82ad978ba101 224 // RK4 (Prediction)
SMART_CLEO 0:82ad978ba101 225 p_RK4 = RK4((double)deltat,ang_rad,gyro_rad);
SMART_CLEO 0:82ad978ba101 226 for( int i=0;i<N;i++ )
SMART_CLEO 0:82ad978ba101 227 {
SMART_CLEO 0:82ad978ba101 228 ang_rad[i] = *p_RK4;
SMART_CLEO 0:82ad978ba101 229 p_RK4++;
SMART_CLEO 0:82ad978ba101 230 }
SMART_CLEO 0:82ad978ba101 231
SMART_CLEO 0:82ad978ba101 232 // EKF (Correction)
SMART_CLEO 0:82ad978ba101 233 EKF_predict(ang_rad,gyro_rad);
SMART_CLEO 0:82ad978ba101 234 correct_loop++;
SMART_CLEO 0:82ad978ba101 235 if (correct_loop>=10){
SMART_CLEO 0:82ad978ba101 236 p_EKF = EKF_correct(ang_rad,gyro_rad,zLWMA);
SMART_CLEO 0:82ad978ba101 237 for(int i=0; i<N; i++)
SMART_CLEO 0:82ad978ba101 238 {
SMART_CLEO 0:82ad978ba101 239 ang_rad[i] = *p_EKF;
SMART_CLEO 0:82ad978ba101 240 p_EKF++;
SMART_CLEO 0:82ad978ba101 241 }
SMART_CLEO 0:82ad978ba101 242 roll = ang_rad[0] * 180 / PI;
SMART_CLEO 0:82ad978ba101 243 pitch = ang_rad[1] * 180 / PI;
SMART_CLEO 0:82ad978ba101 244 //yaw = ang_rad[2] * 180 / PI;
SMART_CLEO 0:82ad978ba101 245 yaw = -yaw_an;
SMART_CLEO 0:82ad978ba101 246
SMART_CLEO 0:82ad978ba101 247 correct_loop = 0;
SMART_CLEO 0:82ad978ba101 248 }
SMART_CLEO 0:82ad978ba101 249 }
SMART_CLEO 0:82ad978ba101 250 else if(filter_flag == 2)
SMART_CLEO 0:82ad978ba101 251 {
SMART_CLEO 0:82ad978ba101 252 double pitchAcc, rollAcc, yawmag, xh, yh;
SMART_CLEO 0:82ad978ba101 253
SMART_CLEO 0:82ad978ba101 254 /* Integrate the gyro data(deg/s) over time to get angle */
SMART_CLEO 0:82ad978ba101 255 angle[1] += Gyro[0] * (double)deltat; // Angle around the X-axis
SMART_CLEO 0:82ad978ba101 256 angle[0] -= Gyro[1] * (double)deltat; // Angle around the Y-axis
SMART_CLEO 0:82ad978ba101 257 //angle[2] -= Gyro[2] * (double)deltat; // Angle around the Z-axis
SMART_CLEO 0:82ad978ba101 258
SMART_CLEO 0:82ad978ba101 259 /* Turning around the X-axis results in a vector on the Y-axis
SMART_CLEO 0:82ad978ba101 260 whereas turning around the Y-axis results in a vector on the X-axis. */
SMART_CLEO 0:82ad978ba101 261 pitchAcc = atan2f(Accel[1], Accel[2])*180/PI;
SMART_CLEO 0:82ad978ba101 262 rollAcc = atan2f(Accel[0], Accel[2])*180/PI;
SMART_CLEO 0:82ad978ba101 263
SMART_CLEO 0:82ad978ba101 264 /* Apply Complementary Filter */
SMART_CLEO 0:82ad978ba101 265 angle[1] = angle[1] * 0.98 + pitchAcc * 0.02;
SMART_CLEO 0:82ad978ba101 266 angle[0] = angle[0] * 0.98 + rollAcc * 0.02;
SMART_CLEO 0:82ad978ba101 267
SMART_CLEO 0:82ad978ba101 268 //xh = Mag[0] * cos(angle[0]*PI/180) - Mag[2] * sin(angle[0]*PI/180);
SMART_CLEO 0:82ad978ba101 269 //yh = Mag[0] * sin(angle[1]*PI/180) * sin(angle[0]*PI/180) + Mag[1] * cos(angle[1]*PI/180) - Mag[2] * sin(angle[1]*PI/180) * cos(angle[0]*PI/180);
SMART_CLEO 0:82ad978ba101 270 //yawmag = -atan2(yh, xh);
SMART_CLEO 0:82ad978ba101 271
SMART_CLEO 0:82ad978ba101 272 //angle[2] = angle[2] * 0.98 + yawmag * 180 / PI * 0.02;
SMART_CLEO 0:82ad978ba101 273 }
SMART_CLEO 0:82ad978ba101 274
SMART_CLEO 0:82ad978ba101 275 // Serial print and/or display at 0.5 s rate independent of data rates
SMART_CLEO 0:82ad978ba101 276 delt_t = t.read_ms() - count;
SMART_CLEO 0:82ad978ba101 277 if (delt_t > 100) { // update LCD once per half-second independent of read rate
SMART_CLEO 0:82ad978ba101 278
SMART_CLEO 0:82ad978ba101 279 if(filter_pre != filter_flag)
SMART_CLEO 0:82ad978ba101 280 {
SMART_CLEO 0:82ad978ba101 281 filter_pre = filter_flag;
SMART_CLEO 0:82ad978ba101 282 lcd.locate(15, 1);
SMART_CLEO 0:82ad978ba101 283 lcd.printf("%d", filter_pre);
SMART_CLEO 0:82ad978ba101 284 }
SMART_CLEO 0:82ad978ba101 285 /* pc.printf("ax = %f", 1000*ax);
SMART_CLEO 0:82ad978ba101 286 pc.printf(" ay = %f", 1000*ay);
SMART_CLEO 0:82ad978ba101 287 pc.printf(" az = %f mg\n\r", 1000*az);
SMART_CLEO 0:82ad978ba101 288
SMART_CLEO 0:82ad978ba101 289 pc.printf("gx = %f", gx);
SMART_CLEO 0:82ad978ba101 290 pc.printf(" gy = %f", gy);
SMART_CLEO 0:82ad978ba101 291 pc.printf(" gz = %f deg/s\n\r", gz);
SMART_CLEO 0:82ad978ba101 292
SMART_CLEO 0:82ad978ba101 293 pc.printf("mx = %f", mx);
SMART_CLEO 0:82ad978ba101 294 pc.printf(" my = %f", my);
SMART_CLEO 0:82ad978ba101 295 pc.printf(" mz = %f mG\n\r", mz);
SMART_CLEO 0:82ad978ba101 296 */
SMART_CLEO 0:82ad978ba101 297 /* tempCount = mpu9250.readTempData(); // Read the adc values
SMART_CLEO 0:82ad978ba101 298 temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
SMART_CLEO 0:82ad978ba101 299 pc.printf(" temperature = %f C\n\r", temperature);
SMART_CLEO 0:82ad978ba101 300 */
SMART_CLEO 0:82ad978ba101 301 /* pc.printf("q0 = %f\n\r", q[0]);
SMART_CLEO 0:82ad978ba101 302 pc.printf("q1 = %f\n\r", q[1]);
SMART_CLEO 0:82ad978ba101 303 pc.printf("q2 = %f\n\r", q[2]);
SMART_CLEO 0:82ad978ba101 304 pc.printf("q3 = %f\n\r", q[3]);
SMART_CLEO 0:82ad978ba101 305 */
SMART_CLEO 0:82ad978ba101 306 /* lcd.clear();
SMART_CLEO 0:82ad978ba101 307 lcd.printString("MPU9250", 0, 0);
SMART_CLEO 0:82ad978ba101 308 lcd.printString("x y z", 0, 1);
SMART_CLEO 0:82ad978ba101 309 sprintf(buffer, "%d %d %d mg", (int)(1000.0f*ax), (int)(1000.0f*ay), (int)(1000.0f*az));
SMART_CLEO 0:82ad978ba101 310 lcd.printString(buffer, 0, 2);
SMART_CLEO 0:82ad978ba101 311 sprintf(buffer, "%d %d %d deg/s", (int)gx, (int)gy, (int)gz);
SMART_CLEO 0:82ad978ba101 312 lcd.printString(buffer, 0, 3);
SMART_CLEO 0:82ad978ba101 313 sprintf(buffer, "%d %d %d mG", (int)mx, (int)my, (int)mz);
SMART_CLEO 0:82ad978ba101 314 lcd.printString(buffer, 0, 4);
SMART_CLEO 0:82ad978ba101 315 */
SMART_CLEO 0:82ad978ba101 316 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
SMART_CLEO 0:82ad978ba101 317 // In this coordinate system, the positive z-axis is down toward Earth.
SMART_CLEO 0:82ad978ba101 318 // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
SMART_CLEO 0:82ad978ba101 319 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
SMART_CLEO 0:82ad978ba101 320 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
SMART_CLEO 0:82ad978ba101 321 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
SMART_CLEO 0:82ad978ba101 322 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
SMART_CLEO 0:82ad978ba101 323 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
SMART_CLEO 0:82ad978ba101 324 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
SMART_CLEO 0:82ad978ba101 325 if(filter_flag == 0)
SMART_CLEO 0:82ad978ba101 326 {
SMART_CLEO 0:82ad978ba101 327 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
SMART_CLEO 0:82ad978ba101 328 roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
SMART_CLEO 0:82ad978ba101 329 pitch *= 180.0f / PI;
SMART_CLEO 0:82ad978ba101 330 //yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
SMART_CLEO 0:82ad978ba101 331 roll *= 180.0f / PI;
SMART_CLEO 0:82ad978ba101 332 //yaw = yaw * 0.02 + yaw_an * 0.98;
SMART_CLEO 0:82ad978ba101 333 pitch = -pitch;
SMART_CLEO 0:82ad978ba101 334 yaw = -yaw_an;
SMART_CLEO 0:82ad978ba101 335 }
SMART_CLEO 0:82ad978ba101 336 else if(filter_flag == 2)
SMART_CLEO 0:82ad978ba101 337 {
SMART_CLEO 0:82ad978ba101 338 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
SMART_CLEO 0:82ad978ba101 339 yaw *= 180.0f / PI;
SMART_CLEO 0:82ad978ba101 340 roll = angle[1];
SMART_CLEO 0:82ad978ba101 341 pitch = angle[0];
SMART_CLEO 0:82ad978ba101 342 yaw = yaw * 0.02 + yaw_an * 0.98;
SMART_CLEO 0:82ad978ba101 343 yaw = -yaw;
SMART_CLEO 0:82ad978ba101 344 }
SMART_CLEO 0:82ad978ba101 345 // pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
SMART_CLEO 0:82ad978ba101 346 // pc.printf("average rate = %f\n\r", (float) sumCount/sum);
SMART_CLEO 0:82ad978ba101 347 // sprintf(buffer, "YPR: %f %f %f", yaw, pitch, roll);
SMART_CLEO 0:82ad978ba101 348 // lcd.printString(buffer, 0, 4);
SMART_CLEO 0:82ad978ba101 349 // sprintf(buffer, "rate = %f", (float) sumCount/sum);
SMART_CLEO 0:82ad978ba101 350 // lcd.printString(buffer, 0, 5);
SMART_CLEO 0:82ad978ba101 351 Buffer[0] = 0x76;
SMART_CLEO 0:82ad978ba101 352 Buffer[1] = 0x02;
SMART_CLEO 0:82ad978ba101 353 Buffer[2] = 0x01;
SMART_CLEO 0:82ad978ba101 354 Buffer[3] = 24;
SMART_CLEO 0:82ad978ba101 355 Data_Tr.data16 = (int16_t)gx;
SMART_CLEO 0:82ad978ba101 356 Buffer[4] = Data_Tr.data8[1];
SMART_CLEO 0:82ad978ba101 357 Buffer[5] = Data_Tr.data8[0];
SMART_CLEO 0:82ad978ba101 358 Data_Tr.data16 = (int16_t)gy;
SMART_CLEO 0:82ad978ba101 359 Buffer[6] = Data_Tr.data8[1];
SMART_CLEO 0:82ad978ba101 360 Buffer[7] = Data_Tr.data8[0];
SMART_CLEO 0:82ad978ba101 361 Data_Tr.data16 = (int16_t)gz;
SMART_CLEO 0:82ad978ba101 362 Buffer[8] = Data_Tr.data8[1];
SMART_CLEO 0:82ad978ba101 363 Buffer[9] = Data_Tr.data8[0];
SMART_CLEO 0:82ad978ba101 364 Data_Tr.data16 = (int16_t)(ax * 1000);
SMART_CLEO 0:82ad978ba101 365 Buffer[10] = Data_Tr.data8[1];
SMART_CLEO 0:82ad978ba101 366 Buffer[11] = Data_Tr.data8[0];
SMART_CLEO 0:82ad978ba101 367 Data_Tr.data16 = (int16_t)(ay * 1000);
SMART_CLEO 0:82ad978ba101 368 Buffer[12] = Data_Tr.data8[1];
SMART_CLEO 0:82ad978ba101 369 Buffer[13] = Data_Tr.data8[0];
SMART_CLEO 0:82ad978ba101 370 Data_Tr.data16 = (int16_t)(az * 1000);
SMART_CLEO 0:82ad978ba101 371 Buffer[14] = Data_Tr.data8[1];
SMART_CLEO 0:82ad978ba101 372 Buffer[15] = Data_Tr.data8[0];
SMART_CLEO 0:82ad978ba101 373 Data_Tr.data16 = (int16_t)mx;
SMART_CLEO 0:82ad978ba101 374 Buffer[16] = Data_Tr.data8[1];
SMART_CLEO 0:82ad978ba101 375 Buffer[17] = Data_Tr.data8[0];
SMART_CLEO 0:82ad978ba101 376 Data_Tr.data16 = (int16_t)my;
SMART_CLEO 0:82ad978ba101 377 Buffer[18] = Data_Tr.data8[1];
SMART_CLEO 0:82ad978ba101 378 Buffer[19] = Data_Tr.data8[0];
SMART_CLEO 0:82ad978ba101 379 Data_Tr.data16 = (int16_t)mz;
SMART_CLEO 0:82ad978ba101 380 Buffer[20] = Data_Tr.data8[1];
SMART_CLEO 0:82ad978ba101 381 Buffer[21] = Data_Tr.data8[0];
SMART_CLEO 0:82ad978ba101 382 Data_Tr.data16 = (int16_t)yaw;
SMART_CLEO 0:82ad978ba101 383 Buffer[22] = Data_Tr.data8[1];
SMART_CLEO 0:82ad978ba101 384 Buffer[23] = Data_Tr.data8[0];
SMART_CLEO 0:82ad978ba101 385 Data_Tr.data16 = (int16_t)roll;
SMART_CLEO 0:82ad978ba101 386 Buffer[24] = Data_Tr.data8[1];
SMART_CLEO 0:82ad978ba101 387 Buffer[25] = Data_Tr.data8[0];
SMART_CLEO 0:82ad978ba101 388 Data_Tr.data16 = (int16_t)pitch;
SMART_CLEO 0:82ad978ba101 389 Buffer[26] = Data_Tr.data8[1];
SMART_CLEO 0:82ad978ba101 390 Buffer[27] = Data_Tr.data8[0];
SMART_CLEO 0:82ad978ba101 391 Buffer[28] = 0x3E;
SMART_CLEO 0:82ad978ba101 392
SMART_CLEO 0:82ad978ba101 393 for(int i=0; i<29; i++)
SMART_CLEO 0:82ad978ba101 394 SerialUART.putc(Buffer[i]);
SMART_CLEO 0:82ad978ba101 395
SMART_CLEO 0:82ad978ba101 396 myled= !myled;
SMART_CLEO 0:82ad978ba101 397 count = t.read_ms();
SMART_CLEO 0:82ad978ba101 398
SMART_CLEO 0:82ad978ba101 399 if(count > 1<<21) {
SMART_CLEO 0:82ad978ba101 400 t.stop();
SMART_CLEO 0:82ad978ba101 401 t.reset();
SMART_CLEO 0:82ad978ba101 402 t.start(); // start the timer over again if ~30 minutes has passed
SMART_CLEO 0:82ad978ba101 403 count = 0;
SMART_CLEO 0:82ad978ba101 404 deltat= 0;
SMART_CLEO 0:82ad978ba101 405 lastUpdate = t.read_us();
SMART_CLEO 0:82ad978ba101 406 }
SMART_CLEO 0:82ad978ba101 407 sum = 0;
SMART_CLEO 0:82ad978ba101 408 sumCount = 0;
SMART_CLEO 0:82ad978ba101 409 }
SMART_CLEO 0:82ad978ba101 410 }
SMART_CLEO 0:82ad978ba101 411 }
SMART_CLEO 0:82ad978ba101 412
SMART_CLEO 0:82ad978ba101 413 void SerialUARTRX_ISR(void)
SMART_CLEO 0:82ad978ba101 414 {
SMART_CLEO 0:82ad978ba101 415 static uint8_t RX_count = 0, RX_Len = 32, RX_Status = 0;
SMART_CLEO 0:82ad978ba101 416 uint8_t rx_da = SerialUART.getc();
SMART_CLEO 0:82ad978ba101 417 switch(RX_Status)
SMART_CLEO 0:82ad978ba101 418 {
SMART_CLEO 0:82ad978ba101 419 case 0:
SMART_CLEO 0:82ad978ba101 420 if(rx_da == 0x76)
SMART_CLEO 0:82ad978ba101 421 {
SMART_CLEO 0:82ad978ba101 422 RX_BUF.STA = rx_da;
SMART_CLEO 0:82ad978ba101 423 RX_Status++;
SMART_CLEO 0:82ad978ba101 424 }
SMART_CLEO 0:82ad978ba101 425 break;
SMART_CLEO 0:82ad978ba101 426 case 1:
SMART_CLEO 0:82ad978ba101 427 RX_BUF.MODE = rx_da;
SMART_CLEO 0:82ad978ba101 428 RX_Status++;
SMART_CLEO 0:82ad978ba101 429 break;
SMART_CLEO 0:82ad978ba101 430 case 2:
SMART_CLEO 0:82ad978ba101 431 RX_BUF.CMD = rx_da;
SMART_CLEO 0:82ad978ba101 432 RX_Status++;
SMART_CLEO 0:82ad978ba101 433 break;
SMART_CLEO 0:82ad978ba101 434 case 3:
SMART_CLEO 0:82ad978ba101 435 RX_BUF.LEN = rx_da;
SMART_CLEO 0:82ad978ba101 436 RX_Len = RX_BUF.LEN;
SMART_CLEO 0:82ad978ba101 437 RX_Status++;
SMART_CLEO 0:82ad978ba101 438 if(RX_Len == 0)
SMART_CLEO 0:82ad978ba101 439 RX_Status++;
SMART_CLEO 0:82ad978ba101 440 break;
SMART_CLEO 0:82ad978ba101 441 case 4:
SMART_CLEO 0:82ad978ba101 442 RX_BUF.DATA[RX_count] = rx_da;
SMART_CLEO 0:82ad978ba101 443 RX_count++;
SMART_CLEO 0:82ad978ba101 444 if(RX_count == RX_Len)
SMART_CLEO 0:82ad978ba101 445 {
SMART_CLEO 0:82ad978ba101 446 RX_Status++;
SMART_CLEO 0:82ad978ba101 447 RX_count = 0;
SMART_CLEO 0:82ad978ba101 448 RX_Len = 32;
SMART_CLEO 0:82ad978ba101 449 }
SMART_CLEO 0:82ad978ba101 450 break;
SMART_CLEO 0:82ad978ba101 451 case 5:
SMART_CLEO 0:82ad978ba101 452 if(rx_da == 0x3E)
SMART_CLEO 0:82ad978ba101 453 {
SMART_CLEO 0:82ad978ba101 454 RX_BUF.END = rx_da;
SMART_CLEO 0:82ad978ba101 455 RX_Status = 0;
SMART_CLEO 0:82ad978ba101 456 switch(RX_BUF.MODE)
SMART_CLEO 0:82ad978ba101 457 {
SMART_CLEO 0:82ad978ba101 458 case 0x05:
SMART_CLEO 0:82ad978ba101 459 filter_flag = RX_BUF.CMD;
SMART_CLEO 0:82ad978ba101 460 break;
SMART_CLEO 0:82ad978ba101 461 }
SMART_CLEO 0:82ad978ba101 462 }
SMART_CLEO 0:82ad978ba101 463 break;
SMART_CLEO 0:82ad978ba101 464 }
SMART_CLEO 0:82ad978ba101 465 }
SMART_CLEO 0:82ad978ba101 466
SMART_CLEO 0:82ad978ba101 467 double* EKF_predict( double y[N], double x[N] ){
SMART_CLEO 0:82ad978ba101 468 // x = F * x;
SMART_CLEO 0:82ad978ba101 469 // P = F * P * F' + G * Q * G';
SMART_CLEO 0:82ad978ba101 470
SMART_CLEO 0:82ad978ba101 471 //double Q[N][N] = { {0.1, 0, 0}, {0, 0.1, 0}, {0, 0, 0.1} };
SMART_CLEO 0:82ad978ba101 472 double Q[N][N] = { {0.00263, 0, 0}, {0, 0.00373, 0}, {0, 0, 0.00509} };
SMART_CLEO 0:82ad978ba101 473
SMART_CLEO 0:82ad978ba101 474 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}};
SMART_CLEO 0:82ad978ba101 475 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}};
SMART_CLEO 0:82ad978ba101 476 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])}};
SMART_CLEO 0:82ad978ba101 477 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])}};
SMART_CLEO 0:82ad978ba101 478
SMART_CLEO 0:82ad978ba101 479 double FPF[N][N] = {0};
SMART_CLEO 0:82ad978ba101 480 double GQG[N][N] = {0};
SMART_CLEO 0:82ad978ba101 481
SMART_CLEO 0:82ad978ba101 482 double FP[N][N] = {0};
SMART_CLEO 0:82ad978ba101 483 double GQ[N][N] = {0};
SMART_CLEO 0:82ad978ba101 484
SMART_CLEO 0:82ad978ba101 485 for( int i=0;i<N;i++ ){
SMART_CLEO 0:82ad978ba101 486 for( int j=0;j<N;j++ ){
SMART_CLEO 0:82ad978ba101 487 for( int k=0;k<N;k++ ){
SMART_CLEO 0:82ad978ba101 488 FP[i][j] += Fjac[i][k]*P[k][j];
SMART_CLEO 0:82ad978ba101 489 GQ[i][j] += Gjac[i][k]*Q[k][j];
SMART_CLEO 0:82ad978ba101 490
SMART_CLEO 0:82ad978ba101 491 }
SMART_CLEO 0:82ad978ba101 492 }
SMART_CLEO 0:82ad978ba101 493 }
SMART_CLEO 0:82ad978ba101 494
SMART_CLEO 0:82ad978ba101 495 for( int i=0;i<N;i++ ){
SMART_CLEO 0:82ad978ba101 496 for( int j=0;j<N;j++ ){
SMART_CLEO 0:82ad978ba101 497 for( int k=0;k<N;k++ ){
SMART_CLEO 0:82ad978ba101 498 FPF[i][j] += FP[i][k]*Fjac_t[k][j];
SMART_CLEO 0:82ad978ba101 499 GQG[i][j] += GQ[i][k]*Gjac_t[k][j];
SMART_CLEO 0:82ad978ba101 500 }
SMART_CLEO 0:82ad978ba101 501 }
SMART_CLEO 0:82ad978ba101 502 }
SMART_CLEO 0:82ad978ba101 503 for( int i=0;i<N;i++ ){
SMART_CLEO 0:82ad978ba101 504 for( int j=0;j<N;j++ ){
SMART_CLEO 0:82ad978ba101 505 P[i][j] = FPF[i][j]+GQG[i][j];
SMART_CLEO 0:82ad978ba101 506 }
SMART_CLEO 0:82ad978ba101 507 }
SMART_CLEO 0:82ad978ba101 508
SMART_CLEO 0:82ad978ba101 509 return 0;
SMART_CLEO 0:82ad978ba101 510
SMART_CLEO 0:82ad978ba101 511 }
SMART_CLEO 0:82ad978ba101 512
SMART_CLEO 0:82ad978ba101 513 double* EKF_correct( double y[N], double x[N], double z[N] ){
SMART_CLEO 0:82ad978ba101 514 // K = P * H' / ( H * P * H' + R )
SMART_CLEO 0:82ad978ba101 515 // x = x + K * ( yobs(t) - H * x )
SMART_CLEO 0:82ad978ba101 516 // P = P - K * H * P
SMART_CLEO 0:82ad978ba101 517
SMART_CLEO 0:82ad978ba101 518 //double R[N][N] = { {0.15, 0, 0}, {0, 0.15, 0}, {0, 0, 0.15} };
SMART_CLEO 0:82ad978ba101 519 double R[N][N] = { {0.00015, 0, 0}, {0, 0.00016, 0}, {0, 0, 0.00032} };
SMART_CLEO 0:82ad978ba101 520
SMART_CLEO 0:82ad978ba101 521 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}};
SMART_CLEO 0:82ad978ba101 522 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}};
SMART_CLEO 0:82ad978ba101 523 double K[N][N] = {0}; // Kalman gain
SMART_CLEO 0:82ad978ba101 524 double K_deno[N][N] = {0}; // Denominator of the kalman gain
SMART_CLEO 0:82ad978ba101 525 double det_K_deno_inv = 0;
SMART_CLEO 0:82ad978ba101 526 double K_deno_inv[N][N] = {0};
SMART_CLEO 0:82ad978ba101 527 double HPH[N][N] = {0};
SMART_CLEO 0:82ad978ba101 528 double HP[N][N] = {0};
SMART_CLEO 0:82ad978ba101 529 double PH[N][N] = {0};
SMART_CLEO 0:82ad978ba101 530 double KHP[N][N] = {0};
SMART_CLEO 0:82ad978ba101 531
SMART_CLEO 0:82ad978ba101 532 double Hx[N] = {0};
SMART_CLEO 0:82ad978ba101 533 double z_Hx[N] = {0};
SMART_CLEO 0:82ad978ba101 534 double Kz_Hx[N] = {0};
SMART_CLEO 0:82ad978ba101 535
SMART_CLEO 0:82ad978ba101 536 double* py = y;
SMART_CLEO 0:82ad978ba101 537
SMART_CLEO 0:82ad978ba101 538 // Kalman gain
SMART_CLEO 0:82ad978ba101 539 for( int i=0;i<N;i++ ){
SMART_CLEO 0:82ad978ba101 540 for( int j=0;j<N;j++ ){
SMART_CLEO 0:82ad978ba101 541 for( int k=0;k<N;k++ ){
SMART_CLEO 0:82ad978ba101 542 HP[i][j] += Hjac[i][k]*P[k][j];
SMART_CLEO 0:82ad978ba101 543 PH[i][j] += P[i][k]*Hjac_t[k][j];
SMART_CLEO 0:82ad978ba101 544 }
SMART_CLEO 0:82ad978ba101 545 }
SMART_CLEO 0:82ad978ba101 546 }
SMART_CLEO 0:82ad978ba101 547 for( int i=0;i<N;i++ ){
SMART_CLEO 0:82ad978ba101 548 for( int j=0;j<N;j++ ){
SMART_CLEO 0:82ad978ba101 549 for( int k=0;k<N;k++ ){
SMART_CLEO 0:82ad978ba101 550 HPH[i][j] += HP[i][k]*Hjac_t[k][j];
SMART_CLEO 0:82ad978ba101 551 }
SMART_CLEO 0:82ad978ba101 552 }
SMART_CLEO 0:82ad978ba101 553 }
SMART_CLEO 0:82ad978ba101 554 for( int i=0;i<N;i++ ){
SMART_CLEO 0:82ad978ba101 555 for( int j=0;j<N;j++ ){
SMART_CLEO 0:82ad978ba101 556 K_deno[i][j] = HPH[i][j]+R[i][j];
SMART_CLEO 0:82ad978ba101 557 }
SMART_CLEO 0:82ad978ba101 558 }
SMART_CLEO 0:82ad978ba101 559 // Inverce matrix
SMART_CLEO 0:82ad978ba101 560 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];
SMART_CLEO 0:82ad978ba101 561 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;
SMART_CLEO 0:82ad978ba101 562 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;
SMART_CLEO 0:82ad978ba101 563 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;
SMART_CLEO 0:82ad978ba101 564 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;
SMART_CLEO 0:82ad978ba101 565 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;
SMART_CLEO 0:82ad978ba101 566 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;
SMART_CLEO 0:82ad978ba101 567 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;
SMART_CLEO 0:82ad978ba101 568 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;
SMART_CLEO 0:82ad978ba101 569 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;
SMART_CLEO 0:82ad978ba101 570
SMART_CLEO 0:82ad978ba101 571 for( int i=0;i<N;i++ ){
SMART_CLEO 0:82ad978ba101 572 for( int j=0;j<N;j++ ){
SMART_CLEO 0:82ad978ba101 573 for( int k=0;k<N;k++ ){
SMART_CLEO 0:82ad978ba101 574 K[i][j] += PH[i][k]*K_deno_inv[k][j];
SMART_CLEO 0:82ad978ba101 575 }
SMART_CLEO 0:82ad978ba101 576 }
SMART_CLEO 0:82ad978ba101 577 }
SMART_CLEO 0:82ad978ba101 578
SMART_CLEO 0:82ad978ba101 579 // Filtering
SMART_CLEO 0:82ad978ba101 580 for( int i=0;i<N;i++ ){
SMART_CLEO 0:82ad978ba101 581 for( int j=0;j<N;j++ ){
SMART_CLEO 0:82ad978ba101 582 Hx[i] += Hjac[i][j]*y[j];
SMART_CLEO 0:82ad978ba101 583 }
SMART_CLEO 0:82ad978ba101 584 }
SMART_CLEO 0:82ad978ba101 585 for( int i=0;i<N;i++ ){
SMART_CLEO 0:82ad978ba101 586 z_Hx[i] = z[i]-Hx[i];
SMART_CLEO 0:82ad978ba101 587 }
SMART_CLEO 0:82ad978ba101 588 for( int i=0;i<N;i++ ){
SMART_CLEO 0:82ad978ba101 589 for( int j=0;j<N;j++ ){
SMART_CLEO 0:82ad978ba101 590 Kz_Hx[i] += K[i][j]*z_Hx[j];
SMART_CLEO 0:82ad978ba101 591 }
SMART_CLEO 0:82ad978ba101 592 }
SMART_CLEO 0:82ad978ba101 593 for( int i=0;i<N;i++ ){
SMART_CLEO 0:82ad978ba101 594 y[i] = y[i]+Kz_Hx[i];
SMART_CLEO 0:82ad978ba101 595 }
SMART_CLEO 0:82ad978ba101 596
SMART_CLEO 0:82ad978ba101 597 // Covarience
SMART_CLEO 0:82ad978ba101 598 for( int i=0;i<N;i++ ){
SMART_CLEO 0:82ad978ba101 599 for( int j=0;j<N;j++ ){
SMART_CLEO 0:82ad978ba101 600 for( int k=0;k<N;k++ ){
SMART_CLEO 0:82ad978ba101 601 KHP[i][j] += K[i][k]*HP[k][j];
SMART_CLEO 0:82ad978ba101 602 }
SMART_CLEO 0:82ad978ba101 603 }
SMART_CLEO 0:82ad978ba101 604 }
SMART_CLEO 0:82ad978ba101 605 for( int i=0;i<N;i++ ){
SMART_CLEO 0:82ad978ba101 606 for( int j=0;j<N;j++ ){
SMART_CLEO 0:82ad978ba101 607 P[i][j] = P[i][j]-KHP[i][j];
SMART_CLEO 0:82ad978ba101 608 }
SMART_CLEO 0:82ad978ba101 609 }
SMART_CLEO 0:82ad978ba101 610
SMART_CLEO 0:82ad978ba101 611 return py;
SMART_CLEO 0:82ad978ba101 612
SMART_CLEO 0:82ad978ba101 613 }
SMART_CLEO 0:82ad978ba101 614
SMART_CLEO 0:82ad978ba101 615 double* LWMA( double z[N] ){
SMART_CLEO 0:82ad978ba101 616
SMART_CLEO 0:82ad978ba101 617 double zLWMA[N] = {0};
SMART_CLEO 0:82ad978ba101 618 double zLWMA_num[N] = {0};
SMART_CLEO 0:82ad978ba101 619 double zLWMA_den = 0;
SMART_CLEO 0:82ad978ba101 620
SMART_CLEO 0:82ad978ba101 621 double* p_zLWMA = zLWMA;
SMART_CLEO 0:82ad978ba101 622
SMART_CLEO 0:82ad978ba101 623 for( int i=1;i<N_LWMA;i++ ){
SMART_CLEO 0:82ad978ba101 624 for( int j=0;j<N;j++ ){
SMART_CLEO 0:82ad978ba101 625 z_buf[j][N_LWMA-i] = z_buf[j][N_LWMA-i-1];
SMART_CLEO 0:82ad978ba101 626 }
SMART_CLEO 0:82ad978ba101 627 }
SMART_CLEO 0:82ad978ba101 628 for( int i=0;i<N;i++ ){
SMART_CLEO 0:82ad978ba101 629 z_buf[i][0] = z[i];
SMART_CLEO 0:82ad978ba101 630 }
SMART_CLEO 0:82ad978ba101 631
SMART_CLEO 0:82ad978ba101 632 for( int i=0;i<N_LWMA;i++ ){
SMART_CLEO 0:82ad978ba101 633 for( int j=0;j<N;j++ ){
SMART_CLEO 0:82ad978ba101 634 zLWMA_num[j] += (N_LWMA-i)*z_buf[j][i];
SMART_CLEO 0:82ad978ba101 635 }
SMART_CLEO 0:82ad978ba101 636 zLWMA_den += N_LWMA-i;
SMART_CLEO 0:82ad978ba101 637 }
SMART_CLEO 0:82ad978ba101 638 for( int i=0;i<N;i++ ){
SMART_CLEO 0:82ad978ba101 639 zLWMA[i] = zLWMA_num[i]/zLWMA_den;
SMART_CLEO 0:82ad978ba101 640 }
SMART_CLEO 0:82ad978ba101 641
SMART_CLEO 0:82ad978ba101 642 return p_zLWMA;
SMART_CLEO 0:82ad978ba101 643
SMART_CLEO 0:82ad978ba101 644 }
SMART_CLEO 0:82ad978ba101 645
SMART_CLEO 0:82ad978ba101 646 double* RK4( double dt, double y[N], double x[N] ){
SMART_CLEO 0:82ad978ba101 647
SMART_CLEO 0:82ad978ba101 648 double yBuf[N] = {0};
SMART_CLEO 0:82ad978ba101 649 double k[N][4] = {0};
SMART_CLEO 0:82ad978ba101 650
SMART_CLEO 0:82ad978ba101 651 double* p_y = y;
SMART_CLEO 0:82ad978ba101 652
SMART_CLEO 0:82ad978ba101 653 double* pk1;
SMART_CLEO 0:82ad978ba101 654 double* pk2;
SMART_CLEO 0:82ad978ba101 655 double* pk3;
SMART_CLEO 0:82ad978ba101 656 double* pk4;
SMART_CLEO 0:82ad978ba101 657
SMART_CLEO 0:82ad978ba101 658 for( int i=0;i<N;i++){ yBuf[i] = y[i]; }
SMART_CLEO 0:82ad978ba101 659 pk1 = func (yBuf,x);
SMART_CLEO 0:82ad978ba101 660 for( int i=0;i<N;i++ ){ k[i][0] = *pk1; pk1 = pk1+1; }
SMART_CLEO 0:82ad978ba101 661
SMART_CLEO 0:82ad978ba101 662 for( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][1]; }
SMART_CLEO 0:82ad978ba101 663 pk2 = func (yBuf,x);
SMART_CLEO 0:82ad978ba101 664 for( int i=0;i<N;i++ ){ k[i][1] = *pk2; pk2 = pk2+1; }
SMART_CLEO 0:82ad978ba101 665
SMART_CLEO 0:82ad978ba101 666 for( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][2]; }
SMART_CLEO 0:82ad978ba101 667 pk3 = func (yBuf,x);
SMART_CLEO 0:82ad978ba101 668 for( int i=0;i<N;i++ ){ k[i][2] = *pk3; pk3 = pk3+1; }
SMART_CLEO 0:82ad978ba101 669
SMART_CLEO 0:82ad978ba101 670 for( int i=0;i<N;i++){ yBuf[i] = y[i]+dt*k[i][3]; }
SMART_CLEO 0:82ad978ba101 671 pk4 = func (yBuf,x);
SMART_CLEO 0:82ad978ba101 672 for( int i=0;i<N;i++ ){ k[i][3] = *pk4; pk4 = pk4+1; }
SMART_CLEO 0:82ad978ba101 673
SMART_CLEO 0:82ad978ba101 674 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; }
SMART_CLEO 0:82ad978ba101 675
SMART_CLEO 0:82ad978ba101 676 return p_y;
SMART_CLEO 0:82ad978ba101 677
SMART_CLEO 0:82ad978ba101 678 }
SMART_CLEO 0:82ad978ba101 679
SMART_CLEO 0:82ad978ba101 680 double* func( double y[N], double x[N] ){
SMART_CLEO 0:82ad978ba101 681
SMART_CLEO 0:82ad978ba101 682 double f[N] = {0};
SMART_CLEO 0:82ad978ba101 683 double* p_f = f;
SMART_CLEO 0:82ad978ba101 684
SMART_CLEO 0:82ad978ba101 685 f[0] = x[0]+x[1]*sin(y[0])*tan(y[1])+x[2]*cos(y[0])*tan(y[1]);
SMART_CLEO 0:82ad978ba101 686 f[1] = x[1]*cos(y[0])-x[2]*sin(y[0]);
SMART_CLEO 0:82ad978ba101 687 f[2] = x[1]*sin(y[0])/cos(y[1])+x[2]*cos(y[0])/cos(y[1]);
SMART_CLEO 0:82ad978ba101 688
SMART_CLEO 0:82ad978ba101 689 return p_f;
SMART_CLEO 0:82ad978ba101 690
SMART_CLEO 0:82ad978ba101 691 }