han back
/
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.
Diff: main.cpp
- Revision:
- 0:82ad978ba101
- Child:
- 1:bb8075327fa6
diff -r 000000000000 -r 82ad978ba101 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Sep 28 03:36:54 2017 +0000
@@ -0,0 +1,691 @@
+#include "mbed.h"
+#include "MPU9250.h"
+#include "TextLCD.h"
+
+struct UART_buf
+{
+ uint8_t STA;
+ uint8_t MODE;
+ uint8_t CMD;
+ uint8_t LEN;
+ uint8_t DATA[32];
+ uint8_t END;
+
+};
+
+union Data_DB{
+ int16_t data16;
+ uint8_t data8[2];
+}Data_Tr;
+
+MPU9250 mpu9250;
+
+Serial SerialUART(PA_2, PA_3); // tx, rx
+
+// rs, rw, e, d0-d3
+TextLCD lcd(PB_12, PB_13, PB_14, PB_15, PA_9, PA_10, PA_11);
+
+#define N 3
+#define N_LWMA 10
+
+uint8_t Buffer[37];
+volatile uint8_t Sensor_flag = 0;
+
+UART_buf RX_BUF;
+
+float sum = 0, yaw_an = 0;
+uint32_t sumCount = 0;
+
+Timer t;
+
+double Accel[N], Gyro[N], Mag[N], angle[N] = {0, 0, 0};
+volatile uint8_t filter_flag = 0, filter_pre = 5;
+
+double z_buf[N][N_LWMA] = {0}; // For LWMA
+
+double P[N][N] = {{1,0,0},{0,1,0},{0,0,1}}; // For EKF
+
+double* EKF_predict( double y[N], double x[N] );
+double* EKF_correct( double y[N], double x[N], double z[N] );
+double* LWMA( double z[N] );
+double* RK4( double dt, double y[N], double x[N] );
+double* func( double y[N], double x[N] );
+void SerialUARTRX_ISR(void);
+void Timer_setting(uint8_t cmd, uint8_t value);
+void Sensor_Read(void);
+
+int main()
+{
+ int16_t az_tmp = 0;
+
+ SerialUART.baud(115200);
+
+ i2c.frequency(400000); // use fast (400 kHz) I2C
+
+ int correct_loop = 0;
+
+ double ang_acc[2] = {0,0};
+ double ang_deg[N] = {0,0,0};
+ double gyro_rad[N] = {0,0,0};
+ double ang_rad[N] = {0,0,0};
+ double zLWMA[N] = {0,0,0};
+
+ double* p_RK4;
+ double* p_EKF;
+ double* p_zLWMA;
+
+ for( int i=0;i<N_LWMA;i++ ){ z_buf[2][i] = 1; }
+
+ uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
+
+ if (whoami == 0x71) // WHO_AM_I should always be 0x68
+ {
+ lcd.printf("MPU9250 is 0x%x\n",whoami);
+ lcd.printf(" Connected ");
+
+ wait(1);
+
+ mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
+ mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
+ /*SerialUART.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);
+ SerialUART.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);
+ SerialUART.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);
+ SerialUART.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);
+ SerialUART.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);
+ SerialUART.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]); */
+ mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
+ /*SerialUART.printf("x gyro bias = %f\n\r", gyroBias[0]);
+ SerialUART.printf("y gyro bias = %f\n\r", gyroBias[1]);
+ SerialUART.printf("z gyro bias = %f\n\r", gyroBias[2]);
+ SerialUART.printf("x accel bias = %f\n\r", accelBias[0]);
+ SerialUART.printf("y accel bias = %f\n\r", accelBias[1]);
+ SerialUART.printf("z accel bias = %f\n\r", accelBias[2]);*/
+ wait(2);
+ mpu9250.initMPU9250();
+ //SerialUART.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+ mpu9250.initAK8963(magCalibration);
+ /*SerialUART.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
+ SerialUART.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
+ pSerialUARTc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
+ if(Mscale == 0) SerialUART.printf("Magnetometer resolution = 14 bits\n\r");
+ if(Mscale == 1) SerialUART.printf("Magnetometer resolution = 16 bits\n\r");
+ if(Mmode == 2) SerialUART.printf("Magnetometer ODR = 8 Hz\n\r");
+ if(Mmode == 6) SerialUART.printf("Magnetometer ODR = 100 Hz\n\r");*/
+ wait(1);
+ }
+ else
+ {
+ //SerialUART.printf("Could not connect to MPU9250: \n\r");
+ //SerialUART.printf("%#x \n", whoami);
+
+ lcd.printf("MPU9250 is 0x%x\n",whoami);
+ lcd.printf(" No connection ");
+
+ while(1) ; // Loop forever if communication doesn't happen
+ }
+
+ t.start();
+
+ mpu9250.getAres(); // Get accelerometer sensitivity
+ mpu9250.getGres(); // Get gyro sensitivity
+ mpu9250.getMres(); // Get magnetometer sensitivity
+ /* pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
+ pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
+ pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);*/
+ magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
+ magbias[1] = +120.; // User environmental x-axis correction in milliGauss
+ magbias[2] = +125.; // User environmental x-axis correction in milliGauss
+
+ SerialUART.attach(&SerialUARTRX_ISR);
+
+ //Timer_setting(0x06, 200);
+ //Sensor_Timer.attach(&Sensor_Read, 0.005);
+
+ while(1) {
+
+ // If intPin goes high, all data registers have new data
+ if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
+
+ mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
+ // Now we'll calculate the accleration value into actual g's
+ for(int i=0; i<3; i++)
+ {
+ Accel[i] = (float)accelCount[i]*aRes - accelBias[i]; // get actual g value, this depends on scale being set
+ }
+ /*
+ ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
+ ay = (float)accelCount[1]*aRes - accelBias[1];
+ az = (float)accelCount[2]*aRes - accelBias[2]; */
+
+ mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
+ // Calculate the gyro value into actual degrees per second
+ for(int i=0; i<3; i++)
+ {
+ Gyro[i] = (float)gyroCount[i]*gRes - gyroBias[i]; // get actual gyro value, this depends on scale being set
+ }
+ /*
+ gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
+ gy = (float)gyroCount[1]*gRes - gyroBias[1];
+ gz = (float)gyroCount[2]*gRes - gyroBias[2]; */
+
+ mpu9250.readMagData(magCount); // Read the x/y/z adc values
+ // Calculate the magnetometer values in milliGauss
+ // Include factory calibration per data sheet and user environmental corrections
+ for(int i=0; i<3; i++)
+ {
+ Mag[i] = (float)magCount[i]*mRes*magCalibration[i] - magbias[i]; // get actual magnetometer value, this depends on scale being set
+ }
+ /*
+ mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
+ my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
+ mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2]; */
+ }
+
+ Now = t.read_us();
+ deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
+ lastUpdate = Now;
+ az_tmp = Gyro[2];
+ yaw_an += ((float)az_tmp * deltat);
+ sum += deltat;
+ sumCount++;
+
+ // if(lastUpdate - firstUpdate > 10000000.0f) {
+ // beta = 0.04; // decrease filter gain after stabilized
+ // zeta = 0.015; // increasey bias drift gain after stabilized
+ // }
+ if(filter_flag != 1)
+ {
+ // Pass gyro rate as rad/s
+ 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]);
+ 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]);
+ }
+ if(filter_flag == 1)
+ {
+
+ for( int i=0;i<N;i++ ){ gyro_rad[i] = Gyro[i]*PI/180; }
+
+ // Compass yaw
+ /*compass_rad = (double)mag[1]/mag[0];
+ compass_rad = atan(compass_rad);
+ //compass_rad = compass_rad-compass_basis_rad;
+ compass_deg = compass_rad*180/pi;
+ */
+ // LWMA (Observation)
+ p_zLWMA = LWMA(Accel);
+ for( int i=0;i<N;i++ )
+ {
+ zLWMA[i] = *p_zLWMA;
+ p_zLWMA++;
+ }
+ // LWMA angle
+ ang_acc[0] = asin(zLWMA[1])*180/PI;
+ ang_acc[1] = asin(zLWMA[0])*180/PI;
+
+ // RK4 (Prediction)
+ p_RK4 = RK4((double)deltat,ang_rad,gyro_rad);
+ for( int i=0;i<N;i++ )
+ {
+ ang_rad[i] = *p_RK4;
+ p_RK4++;
+ }
+
+ // EKF (Correction)
+ EKF_predict(ang_rad,gyro_rad);
+ correct_loop++;
+ if (correct_loop>=10){
+ p_EKF = EKF_correct(ang_rad,gyro_rad,zLWMA);
+ for(int i=0; i<N; i++)
+ {
+ ang_rad[i] = *p_EKF;
+ p_EKF++;
+ }
+ roll = ang_rad[0] * 180 / PI;
+ pitch = ang_rad[1] * 180 / PI;
+ //yaw = ang_rad[2] * 180 / PI;
+ yaw = -yaw_an;
+
+ correct_loop = 0;
+ }
+ }
+ else if(filter_flag == 2)
+ {
+ double pitchAcc, rollAcc, yawmag, xh, yh;
+
+ /* Integrate the gyro data(deg/s) over time to get angle */
+ angle[1] += Gyro[0] * (double)deltat; // Angle around the X-axis
+ angle[0] -= Gyro[1] * (double)deltat; // Angle around the Y-axis
+ //angle[2] -= Gyro[2] * (double)deltat; // Angle around the Z-axis
+
+ /* Turning around the X-axis results in a vector on the Y-axis
+ whereas turning around the Y-axis results in a vector on the X-axis. */
+ pitchAcc = atan2f(Accel[1], Accel[2])*180/PI;
+ rollAcc = atan2f(Accel[0], Accel[2])*180/PI;
+
+ /* Apply Complementary Filter */
+ angle[1] = angle[1] * 0.98 + pitchAcc * 0.02;
+ angle[0] = angle[0] * 0.98 + rollAcc * 0.02;
+
+ //xh = Mag[0] * cos(angle[0]*PI/180) - Mag[2] * sin(angle[0]*PI/180);
+ //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);
+ //yawmag = -atan2(yh, xh);
+
+ //angle[2] = angle[2] * 0.98 + yawmag * 180 / PI * 0.02;
+ }
+
+ // Serial print and/or display at 0.5 s rate independent of data rates
+ delt_t = t.read_ms() - count;
+ if (delt_t > 100) { // update LCD once per half-second independent of read rate
+
+ if(filter_pre != filter_flag)
+ {
+ filter_pre = filter_flag;
+ lcd.locate(15, 1);
+ lcd.printf("%d", filter_pre);
+ }
+/* pc.printf("ax = %f", 1000*ax);
+ pc.printf(" ay = %f", 1000*ay);
+ pc.printf(" az = %f mg\n\r", 1000*az);
+
+ pc.printf("gx = %f", gx);
+ pc.printf(" gy = %f", gy);
+ pc.printf(" gz = %f deg/s\n\r", gz);
+
+ pc.printf("mx = %f", mx);
+ pc.printf(" my = %f", my);
+ pc.printf(" mz = %f mG\n\r", mz);
+*/
+ /* tempCount = mpu9250.readTempData(); // Read the adc values
+ temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
+ pc.printf(" temperature = %f C\n\r", temperature);
+ */
+/* pc.printf("q0 = %f\n\r", q[0]);
+ pc.printf("q1 = %f\n\r", q[1]);
+ pc.printf("q2 = %f\n\r", q[2]);
+ pc.printf("q3 = %f\n\r", q[3]);
+*/
+ /* lcd.clear();
+ lcd.printString("MPU9250", 0, 0);
+ lcd.printString("x y z", 0, 1);
+ sprintf(buffer, "%d %d %d mg", (int)(1000.0f*ax), (int)(1000.0f*ay), (int)(1000.0f*az));
+ lcd.printString(buffer, 0, 2);
+ sprintf(buffer, "%d %d %d deg/s", (int)gx, (int)gy, (int)gz);
+ lcd.printString(buffer, 0, 3);
+ sprintf(buffer, "%d %d %d mG", (int)mx, (int)my, (int)mz);
+ lcd.printString(buffer, 0, 4);
+ */
+ // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
+ // In this coordinate system, the positive z-axis is down toward Earth.
+ // 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.
+ // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
+ // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
+ // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
+ // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
+ // applied in the correct order which for this configuration is yaw, pitch, and then roll.
+ // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
+ if(filter_flag == 0)
+ {
+ pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+ 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]);
+ pitch *= 180.0f / PI;
+ //yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
+ roll *= 180.0f / PI;
+ //yaw = yaw * 0.02 + yaw_an * 0.98;
+ pitch = -pitch;
+ yaw = -yaw_an;
+ }
+ else if(filter_flag == 2)
+ {
+ 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]);
+ yaw *= 180.0f / PI;
+ roll = angle[1];
+ pitch = angle[0];
+ yaw = yaw * 0.02 + yaw_an * 0.98;
+ yaw = -yaw;
+ }
+// pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
+// pc.printf("average rate = %f\n\r", (float) sumCount/sum);
+ // sprintf(buffer, "YPR: %f %f %f", yaw, pitch, roll);
+ // lcd.printString(buffer, 0, 4);
+ // sprintf(buffer, "rate = %f", (float) sumCount/sum);
+ // lcd.printString(buffer, 0, 5);
+ Buffer[0] = 0x76;
+ Buffer[1] = 0x02;
+ Buffer[2] = 0x01;
+ Buffer[3] = 24;
+ Data_Tr.data16 = (int16_t)gx;
+ Buffer[4] = Data_Tr.data8[1];
+ Buffer[5] = Data_Tr.data8[0];
+ Data_Tr.data16 = (int16_t)gy;
+ Buffer[6] = Data_Tr.data8[1];
+ Buffer[7] = Data_Tr.data8[0];
+ Data_Tr.data16 = (int16_t)gz;
+ Buffer[8] = Data_Tr.data8[1];
+ Buffer[9] = Data_Tr.data8[0];
+ Data_Tr.data16 = (int16_t)(ax * 1000);
+ Buffer[10] = Data_Tr.data8[1];
+ Buffer[11] = Data_Tr.data8[0];
+ Data_Tr.data16 = (int16_t)(ay * 1000);
+ Buffer[12] = Data_Tr.data8[1];
+ Buffer[13] = Data_Tr.data8[0];
+ Data_Tr.data16 = (int16_t)(az * 1000);
+ Buffer[14] = Data_Tr.data8[1];
+ Buffer[15] = Data_Tr.data8[0];
+ Data_Tr.data16 = (int16_t)mx;
+ Buffer[16] = Data_Tr.data8[1];
+ Buffer[17] = Data_Tr.data8[0];
+ Data_Tr.data16 = (int16_t)my;
+ Buffer[18] = Data_Tr.data8[1];
+ Buffer[19] = Data_Tr.data8[0];
+ Data_Tr.data16 = (int16_t)mz;
+ Buffer[20] = Data_Tr.data8[1];
+ Buffer[21] = Data_Tr.data8[0];
+ Data_Tr.data16 = (int16_t)yaw;
+ Buffer[22] = Data_Tr.data8[1];
+ Buffer[23] = Data_Tr.data8[0];
+ Data_Tr.data16 = (int16_t)roll;
+ Buffer[24] = Data_Tr.data8[1];
+ Buffer[25] = Data_Tr.data8[0];
+ Data_Tr.data16 = (int16_t)pitch;
+ Buffer[26] = Data_Tr.data8[1];
+ Buffer[27] = Data_Tr.data8[0];
+ Buffer[28] = 0x3E;
+
+ for(int i=0; i<29; i++)
+ SerialUART.putc(Buffer[i]);
+
+ myled= !myled;
+ count = t.read_ms();
+
+ if(count > 1<<21) {
+ t.stop();
+ t.reset();
+ t.start(); // start the timer over again if ~30 minutes has passed
+ count = 0;
+ deltat= 0;
+ lastUpdate = t.read_us();
+ }
+ sum = 0;
+ sumCount = 0;
+ }
+ }
+}
+
+void SerialUARTRX_ISR(void)
+{
+ static uint8_t RX_count = 0, RX_Len = 32, RX_Status = 0;
+ uint8_t rx_da = SerialUART.getc();
+ switch(RX_Status)
+ {
+ case 0:
+ if(rx_da == 0x76)
+ {
+ RX_BUF.STA = rx_da;
+ RX_Status++;
+ }
+ break;
+ case 1:
+ RX_BUF.MODE = rx_da;
+ RX_Status++;
+ break;
+ case 2:
+ RX_BUF.CMD = rx_da;
+ RX_Status++;
+ break;
+ case 3:
+ RX_BUF.LEN = rx_da;
+ RX_Len = RX_BUF.LEN;
+ RX_Status++;
+ if(RX_Len == 0)
+ RX_Status++;
+ break;
+ case 4:
+ RX_BUF.DATA[RX_count] = rx_da;
+ RX_count++;
+ if(RX_count == RX_Len)
+ {
+ RX_Status++;
+ RX_count = 0;
+ RX_Len = 32;
+ }
+ break;
+ case 5:
+ if(rx_da == 0x3E)
+ {
+ RX_BUF.END = rx_da;
+ RX_Status = 0;
+ switch(RX_BUF.MODE)
+ {
+ case 0x05:
+ filter_flag = RX_BUF.CMD;
+ break;
+ }
+ }
+ break;
+ }
+}
+
+double* EKF_predict( double y[N], double x[N] ){
+ // x = F * x;
+ // P = F * P * F' + G * Q * G';
+
+ //double Q[N][N] = { {0.1, 0, 0}, {0, 0.1, 0}, {0, 0, 0.1} };
+ double Q[N][N] = { {0.00263, 0, 0}, {0, 0.00373, 0}, {0, 0, 0.00509} };
+
+ double Fjac[N][N] = {{x[1]*cos(y[0])*tan(y[1])-x[2]*sin(y[0])*tan(y[1]), x[1]*sin(y[0])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])/(cos(y[1])*cos(y[1])), 0}, {-x[1]*sin(y[0])-x[2]*cos(y[0]), 0, 0}, {x[1]*cos(y[0])/cos(y[1])-x[2]*sin(y[0])/cos(y[1]), x[1]*sin(y[0])*sin(y[1])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])*sin(y[1])/(cos(y[1])*cos(y[1])), 0}};
+ double Fjac_t[N][N] = {{x[1]*cos(y[0])*tan(y[1])-x[2]*sin(y[0])*tan(y[1]), -x[1]*sin(y[0])-x[2]*cos(y[0]), x[1]*cos(y[0])/cos(y[1])-x[2]*sin(y[0])/cos(y[1])}, {x[1]*sin(y[0])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])/(cos(y[1])*cos(y[1])), 0, x[1]*sin(y[0])*sin(y[1])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])*sin(y[1])/(cos(y[1])*cos(y[1]))}, {0, 0, 0}};
+ double Gjac[N][N] = {{1, sin(y[0])*tan(y[1]), cos(y[0])*tan(y[1])}, {0, cos(y[0]), -sin(y[0])}, {0, sin(y[0])/cos(y[1]), cos(y[0])/cos(y[1])}};
+ double Gjac_t[N][N] = {{1, 0, 0}, {sin(y[0])*tan(y[1]), cos(y[0]), sin(y[0])/cos(y[1])}, {cos(y[0])*tan(y[1]), -sin(y[0]), cos(y[0])/cos(y[1])}};
+
+ double FPF[N][N] = {0};
+ double GQG[N][N] = {0};
+
+ double FP[N][N] = {0};
+ double GQ[N][N] = {0};
+
+ for( int i=0;i<N;i++ ){
+ for( int j=0;j<N;j++ ){
+ for( int k=0;k<N;k++ ){
+ FP[i][j] += Fjac[i][k]*P[k][j];
+ GQ[i][j] += Gjac[i][k]*Q[k][j];
+
+ }
+ }
+ }
+
+ for( int i=0;i<N;i++ ){
+ for( int j=0;j<N;j++ ){
+ for( int k=0;k<N;k++ ){
+ FPF[i][j] += FP[i][k]*Fjac_t[k][j];
+ GQG[i][j] += GQ[i][k]*Gjac_t[k][j];
+ }
+ }
+ }
+ for( int i=0;i<N;i++ ){
+ for( int j=0;j<N;j++ ){
+ P[i][j] = FPF[i][j]+GQG[i][j];
+ }
+ }
+
+ return 0;
+
+}
+
+double* EKF_correct( double y[N], double x[N], double z[N] ){
+ // K = P * H' / ( H * P * H' + R )
+ // x = x + K * ( yobs(t) - H * x )
+ // P = P - K * H * P
+
+ //double R[N][N] = { {0.15, 0, 0}, {0, 0.15, 0}, {0, 0, 0.15} };
+ double R[N][N] = { {0.00015, 0, 0}, {0, 0.00016, 0}, {0, 0, 0.00032} };
+
+ double Hjac[N][N] = {{0, cos(y[1]), 0}, {cos(y[0]), 0, 0}, {-sin(y[0])*cos(y[1]), -cos(y[0])*sin(y[1]), 0}};
+ double Hjac_t[N][N] = {{0, cos(y[0]), -sin(y[0])*cos(y[1])}, {cos(y[1]), 0, -cos(y[0])*sin(y[1])}, {0, 0, 0}};
+ double K[N][N] = {0}; // Kalman gain
+ double K_deno[N][N] = {0}; // Denominator of the kalman gain
+ double det_K_deno_inv = 0;
+ double K_deno_inv[N][N] = {0};
+ double HPH[N][N] = {0};
+ double HP[N][N] = {0};
+ double PH[N][N] = {0};
+ double KHP[N][N] = {0};
+
+ double Hx[N] = {0};
+ double z_Hx[N] = {0};
+ double Kz_Hx[N] = {0};
+
+ double* py = y;
+
+ // Kalman gain
+ for( int i=0;i<N;i++ ){
+ for( int j=0;j<N;j++ ){
+ for( int k=0;k<N;k++ ){
+ HP[i][j] += Hjac[i][k]*P[k][j];
+ PH[i][j] += P[i][k]*Hjac_t[k][j];
+ }
+ }
+ }
+ for( int i=0;i<N;i++ ){
+ for( int j=0;j<N;j++ ){
+ for( int k=0;k<N;k++ ){
+ HPH[i][j] += HP[i][k]*Hjac_t[k][j];
+ }
+ }
+ }
+ for( int i=0;i<N;i++ ){
+ for( int j=0;j<N;j++ ){
+ K_deno[i][j] = HPH[i][j]+R[i][j];
+ }
+ }
+ // Inverce matrix
+ det_K_deno_inv = K_deno[0][0]*K_deno[1][1]*K_deno[2][2]+K_deno[1][0]*K_deno[2][1]*K_deno[0][2]+K_deno[2][0]*K_deno[0][1]*K_deno[1][2]-K_deno[0][0]*K_deno[2][1]*K_deno[1][2]-K_deno[2][0]*K_deno[1][1]*K_deno[0][2]-K_deno[1][0]*K_deno[0][1]*K_deno[2][2];
+ K_deno_inv[0][0] = (K_deno[1][1]*K_deno[2][2]-K_deno[1][2]*K_deno[2][1])/det_K_deno_inv;
+ K_deno_inv[0][1] = (K_deno[0][2]*K_deno[2][1]-K_deno[0][1]*K_deno[2][2])/det_K_deno_inv;
+ K_deno_inv[0][2] = (K_deno[0][1]*K_deno[1][2]-K_deno[0][2]*K_deno[1][1])/det_K_deno_inv;
+ K_deno_inv[1][0] = (K_deno[1][2]*K_deno[2][0]-K_deno[1][0]*K_deno[2][2])/det_K_deno_inv;
+ K_deno_inv[1][1] = (K_deno[0][0]*K_deno[2][2]-K_deno[0][2]*K_deno[2][0])/det_K_deno_inv;
+ K_deno_inv[1][2] = (K_deno[0][2]*K_deno[1][0]-K_deno[0][0]*K_deno[1][2])/det_K_deno_inv;
+ K_deno_inv[2][0] = (K_deno[1][0]*K_deno[2][1]-K_deno[1][1]*K_deno[2][0])/det_K_deno_inv;
+ K_deno_inv[2][1] = (K_deno[0][1]*K_deno[2][0]-K_deno[0][0]*K_deno[2][1])/det_K_deno_inv;
+ K_deno_inv[2][2] = (K_deno[0][0]*K_deno[1][1]-K_deno[0][1]*K_deno[1][0])/det_K_deno_inv;
+
+ for( int i=0;i<N;i++ ){
+ for( int j=0;j<N;j++ ){
+ for( int k=0;k<N;k++ ){
+ K[i][j] += PH[i][k]*K_deno_inv[k][j];
+ }
+ }
+ }
+
+ // Filtering
+ for( int i=0;i<N;i++ ){
+ for( int j=0;j<N;j++ ){
+ Hx[i] += Hjac[i][j]*y[j];
+ }
+ }
+ for( int i=0;i<N;i++ ){
+ z_Hx[i] = z[i]-Hx[i];
+ }
+ for( int i=0;i<N;i++ ){
+ for( int j=0;j<N;j++ ){
+ Kz_Hx[i] += K[i][j]*z_Hx[j];
+ }
+ }
+ for( int i=0;i<N;i++ ){
+ y[i] = y[i]+Kz_Hx[i];
+ }
+
+ // Covarience
+ for( int i=0;i<N;i++ ){
+ for( int j=0;j<N;j++ ){
+ for( int k=0;k<N;k++ ){
+ KHP[i][j] += K[i][k]*HP[k][j];
+ }
+ }
+ }
+ for( int i=0;i<N;i++ ){
+ for( int j=0;j<N;j++ ){
+ P[i][j] = P[i][j]-KHP[i][j];
+ }
+ }
+
+ return py;
+
+}
+
+double* LWMA( double z[N] ){
+
+ double zLWMA[N] = {0};
+ double zLWMA_num[N] = {0};
+ double zLWMA_den = 0;
+
+ double* p_zLWMA = zLWMA;
+
+ for( int i=1;i<N_LWMA;i++ ){
+ for( int j=0;j<N;j++ ){
+ z_buf[j][N_LWMA-i] = z_buf[j][N_LWMA-i-1];
+ }
+ }
+ for( int i=0;i<N;i++ ){
+ z_buf[i][0] = z[i];
+ }
+
+ for( int i=0;i<N_LWMA;i++ ){
+ for( int j=0;j<N;j++ ){
+ zLWMA_num[j] += (N_LWMA-i)*z_buf[j][i];
+ }
+ zLWMA_den += N_LWMA-i;
+ }
+ for( int i=0;i<N;i++ ){
+ zLWMA[i] = zLWMA_num[i]/zLWMA_den;
+ }
+
+ return p_zLWMA;
+
+}
+
+double* RK4( double dt, double y[N], double x[N] ){
+
+ double yBuf[N] = {0};
+ double k[N][4] = {0};
+
+ double* p_y = y;
+
+ double* pk1;
+ double* pk2;
+ double* pk3;
+ double* pk4;
+
+ for( int i=0;i<N;i++){ yBuf[i] = y[i]; }
+ pk1 = func (yBuf,x);
+ for( int i=0;i<N;i++ ){ k[i][0] = *pk1; pk1 = pk1+1; }
+
+ for( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][1]; }
+ pk2 = func (yBuf,x);
+ for( int i=0;i<N;i++ ){ k[i][1] = *pk2; pk2 = pk2+1; }
+
+ for( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][2]; }
+ pk3 = func (yBuf,x);
+ for( int i=0;i<N;i++ ){ k[i][2] = *pk3; pk3 = pk3+1; }
+
+ for( int i=0;i<N;i++){ yBuf[i] = y[i]+dt*k[i][3]; }
+ pk4 = func (yBuf,x);
+ for( int i=0;i<N;i++ ){ k[i][3] = *pk4; pk4 = pk4+1; }
+
+ for( int i=0;i<N;i++){ y[i] = y[i]+dt*(k[i][0]+2.0*k[i][1]+2.0*k[i][2]+k[i][3])/6.0; }
+
+ return p_y;
+
+}
+
+double* func( double y[N], double x[N] ){
+
+ double f[N] = {0};
+ double* p_f = f;
+
+ f[0] = x[0]+x[1]*sin(y[0])*tan(y[1])+x[2]*cos(y[0])*tan(y[1]);
+ f[1] = x[1]*cos(y[0])-x[2]*sin(y[0]);
+ f[2] = x[1]*sin(y[0])/cos(y[1])+x[2]*cos(y[0])/cos(y[1]);
+
+ return p_f;
+
+}