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.
main.cpp
- Committer:
- SMART_CLEO
- Date:
- 2017-09-28
- Revision:
- 0:82ad978ba101
- Child:
- 1:bb8075327fa6
File content as of revision 0:82ad978ba101:
#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;
}