Kalman Filter (vi)--Application of Kalman filter: four-element Kalman filter (QKF) C code to realize attitude calculation

0 Introduction

In the practice of Strapdown inertial navigation [6] , we want the gyroscope to be able to obtain very accurate information, or to expect the gyroscope to accurately reflect the real value of the observed (acceleration, magnetic field, etc.)[6,7] , but this process is more or less disturbed by noise, resulting in inaccurate measurements, data fusion and filtering of state variables and observations are necessary to minimize noise disturbance in order to make the gyroscope accurate in state updating.

The most commonly used and most effective method is the non-Kalman filter, which has a good effect on the system of Gaussian model. With the development of computer technology, the computational requirements and complexity ofKalman filter are no longer a hindrance in its application, and are more and more favored by people. Kalman filter is the least variance estimator of linear system, but in practical application, the state model represented by the four-tuple is nonlinear, and one way is to consider using extended Kalman filter (EKF), However, it needs to find an Jacobian matrix in solving the four-tuple differential equation, the computational complexity is impractical for the high-dimensional problem, and the other is to consider the use of nonlinear filtering algorithms, such as the non-trace Kalman filter [5], particle filter, etc., but they are extremely complex and cumbersome computational volume, So that it is difficult to be applied in engineering practice; The Hamilton operator is used to rewrite the four-tuple state equation into linear form, and it is improved when the correlation covariance matrix is computed.

In addition, in the two update process of attitude solving, it is very important to get the angle of the original output angle increment of the gyroscope to be updated, if the effect of noise on the original output can be reduced in the integration process of gyro, the gradual diffusion and amplification of noise will be reduced in the subsequent iterative update. Because the integral of angle and displacement is the integral of time, if improper treatment, it is easy to cause the noise to increase gradually with the continuation of time, so it is difficult to reach the practical application precision.

1. Four USD

2. QKF2.1 status update based on three-time spline interpolation

2.2 Observation Updates

3.C Code Implementation

Initial parameter settings:

The initial four-dollar number is

Here is the C code:

#define ROU_K       0.001#define sigama_1    0.5#define sigama_2    6#define sigama_3    0.01

Float q_k_k[4]={1,0,0,0};float p_k_k[7][7]={5,0,0,0,0,0,0, 0,5,0,0,0,0,0, 0,0,5,0,0,0, 0, 0,0,0,5,0,0,0, 0,0,0,0,5,0,0, 0,0,0,0,0,5,0, 0, 0,0,0,0,0,5};float p_k_k_q[4][4]={5,0,0,0, 0,5,0,0, 0,0,5,0, 0                      , 0,0,5};float u_k_k[3]={0,0,0};float exp_theta_x,exp_theta_y,exp_theta_z;const float I4[4][4]={1,0,0,0,                      0,1,0,0, 0,0,1,0, 0,0,0,1};const float i7[7][7]={1,0,0,0,0,0,0, 0,1,0,0,0,0,0, 0,0,1,0,0,0,0, 0,0,0,1,0,0,0, 0,0,0,  0,1,0,0, 0,0,0,0,0,1,0, 0,0,0,0,0,0,1};float b_k1[3]={0};float r_k1[3]={0};float    C_k1[3]={0};float d_k1[3]={0};void qkf_update2 (void) {float x_k_k[4][3];    float x_k_k[7];    Float x_k1_k[7]={0};  Float x_k1_k1[7]={0};  Float p_k1_k[7][7]={0};    float p_k1_k_q[4][4];    Float p_k1_k1[7][7]={0};    Float p_k1_k_v[4][4]={0};    Float p_k_w[7][7]={0};    float q_k1_k[4];    Float q_k1_k1[4]={0};    float f_k_k[4][4];    float f_k[4][4];    float theta_k[3];    float theta_k_k[3];    float m_k_k[4][4];    float m_k1_k[4][4];    Float h_k1[4][4]={0};    Float h_k1_2[4][7]={0};        Float b_k1[4][4]={0};    Float s_k1_k[4][4]={0};    Float k_k1[7][4]={0};        Float pusai[7][7]={0};    Float temp_77_1[7][7]={0};    Float temp_77_2[7][7]={0};    Float temp_77_3[7][7]={0};    Float temp_77_4[7][7]={0};    Float temp_44_1[4][4]={0};    Float temp_44_2[4][4]={0};    Float temp_44_3[4][4]={0};     Float temp_44_4[4][4]={0};    Float temp_74[7][4]={0};       Float temp_74_2[7][4]={0};    Float temp_47_1[4][7]={0};    float TR;           float temp;    unsigned char i=0,j=0;    for (i=0;i<3;i++) {b_k1[i]=gyro[i];    }/* Below for TIME update */for (i=0;i<4;i++) {x_k_k[i]=q_k_k[i];    }for (i=0;i<3;i++) {x_k_k[i+4]=u_k_k[i];    }//To X_k_k assignment for (i=0;i<3;i++) {theta_k[i]=theta[i];    }//To Theta_k assignment for (i=0;i<3;i++) {theta_k_k[i]=theta_k[i]-two_tim*u_k_k[i];    }//Assign value to Theta_k_k exp_theta_x=exp (0.5*theta_k_k[0]);    Exp_theta_y=exp (0.5*theta_k_k[1]);    Exp_theta_z=exp (0.5*theta_k_k[2]);    F_k_k[0][0]=f_k_k[1][1]=f_k_k[2][2]=f_k_k[3][3]=1;    f_k_k[0][1]=f_k_k[3][2]=1/exp_theta_x;    f_k_k[0][2]=f_k_k[1][3]=1/exp_theta_y;    F_k_k[0][3]=f_k_k[2][1]=1/exp_theta_z;    f_k_k[1][0]=f_k_k[2][3]=exp_theta_x;    f_k_k[2][0]=f_k_k[3][1]=exp_theta_y;                 F_k_k[3][0]=f_k_k[1][2]=exp_theta_z;        Calculates F_k_k for (i=0;i<7;i++) {for (j=0;j<7;j++) {temp_77_1[i][j]=0;    }}//Empty temporary variable.    Temp_77_1[0][0]=temp_77_1[1][1]=temp_77_1[2][2]=1; for (i=0;i<4;i++) {for (j=0;j<4;j++) {temp_77_1[i+3][j+3]=f_k_k[i][j];  }} Matrixmul ((float*) Temp_77_1, (float*) X_k_k, (float*) x_k1_k,7,7,1);    X_k1_k assignment for (i=0;i<4;i++) {q_k1_k[i]=x_k1_k[i];    }//Extract Q_k1_k exp_theta_x=exp from X_k1_k (0.5*theta_k[0]);    Exp_theta_y=exp (0.5*theta_k[1]);    Exp_theta_z=exp (0.5*theta_k[2]);    F_k[0][0]=f_k[1][1]=f_k[2][2]=f_k[3][3]=1;    f_k[0][1]=f_k[3][2]=1/exp_theta_x;    f_k[0][2]=f_k[1][3]=1/exp_theta_y;    F_k[0][3]=f_k[2][1]=1/exp_theta_z;    f_k[1][0]=f_k[2][3]=exp_theta_x;    f_k[2][0]=f_k[3][1]=exp_theta_y;                 F_k[3][0]=f_k[1][2]=exp_theta_z;    Calculation F_k x_k_k[0][0]=x_k_k[1][1]=x_k_k[2][2]=q_k_k[0];    X_K_K[0][1]=X_K_K[3][2]=-1*Q_K_K[2];    X_K_K[0][2]=Q_K_K[1];    X_K_K[1][0]=Q_K_K[2];    X_K_K[1][2]=X_K_K[3][0]=-1*Q_K_K[3];    X_K_K[2][0]=X_K_K[3][1]=-1*Q_K_K[1];                 X_K_K[2][1]=Q_K_K[3];           Assign a value of X_k_k to (i=0;i<7;i++) {for (j=0;j<7;j++) {pusai[i][j]=0;    }} pusai[0][0]=pusai[1][1]=pusai[2][2]=1;        for (i=0;i<4;i++) {for (j=0;j<3;j++) {pusai[i+3][j]=-0.5*two_tim*x_k_k[i][j]; }} for (i=0;i<4;i++) {for (j=0;j<4;j++) {pusai[i+3][j+3]=f_k[        I][J]; }}//To Pusai Assignment Matrixmul ((float*) Q_k_k, (float*) Q_k_k, (float*) temp_44_1,4,1,4)    ;    Matrixadd ((float*) Temp_44_1, (float*) p_k_k_q, (float*) m_k_k,4,4);          TR=M_K_K[0][0]+M_K_K[1][1]+M_K_K[2][2]+M_K_K[3][3];        Calculates M_k_k and TR for (i=0;i<7;i++) {for (j=0;j<7;j++) {p_k_w[i][j]=0; }}//Empty P_k_w P_k_w[0][0]=p_k_w[1][1]=p_k_w[2][2]=sigama_3*sigama_3*two    _tim;           for (i=0;i<4;i++) {for (j=0;j<4;j++) { TEMP_44_1[I][J]=TR*I4[I][J];    }} matrixminus ((float*) Temp_44_1, (float*) M_k_k, (float*) temp_44_2,4,4);    temp=0.25* (Sigama_1*sigama_1+sigama_2*sigama_2*two_tim);        for (i=0;i<4;i++) {for (j=0;j<4;j++) {p_k_w[i+3][j+3]=temp*temp_44_2[i][j];    }}//To P_k_w Assignment Matrixtrans ((float*) Pusai, (float*) temp_77_2,7,7);    Matrixmul ((float*) Pusai, (float*) P_k_k, (float*) temp_77_1,7,7,7);    Matrixmul ((float*) Temp_77_1, (float*) temp_77_2, (float*) temp_77_3,7,7,7); Matrixadd ((float*) Temp_77_3, (float*) P_k_w, (float*) p_k1_k,7,7);        Assign a value of P_k1_k to (i=0;i<4;i++) {for (j=0;j<4;j++) {p_k1_k_q[i][j]=p_k1_k[i][j];    }}//extract p_k1_k_q/* from P_k1_k//R_K1 generate random sequence ....        */For (i=0;i<3;i++) {c_k1[i]=0.5* (b_k1[i]+r_k1[i]);    d_k1[i]=0.5* (B_k1[i]-r_k1[i]); } h_k1[0][0]=h_k1[1][1]=h_k1[2][2]=h_k1[3][3]=0;        for (i=0;i<3;i++) {h_k1[0][i+1]=-1*d_k1[i];    H_k1[i+1][0]=d_k1[i];    } h_k1[1][2]= c_k1[2];    H_K1[1][3]=-1*C_K1[1];    H_K1[2][1]=-1*C_K1[2];    H_k1[2][3]= C_k1[0];    H_k1[3][1]= C_k1[1];         H_K1[3][2]=-1*C_K1[0];        Assign a value of H_k1 to (i=0;i<4;i++) {for (j=0;j<4;j++) {h_k1_2[i][j+3]=h_k1[i][j];    }}//To H_k1_2 Assignment Matrixmul ((float*) Q_k1_k, (float*) Q_k1_k, (float*) temp_44_1,4,1,4);  Matrixadd ((float*) Temp_44_1, (float*) p_k1_k_q, (float*) m_k1_k,4,4);   Calculation M_k1_k tr=m_k1_k[0][0]+m_k1_k[1][1]+m_k1_k[2][2]+m_k1_k[3][3];    calculate TR b_k1[0][0]=b_k1[1][1]=b_k1[2][2]=b_k1[3][3]=0;    B_K1[0][1]=B_K1[3][2]=-1*B_K1[0];    B_K1[0][2]=B_K1[1][3]=-1*B_K1[1];    B_K1[0][3]=B_K1[2][1]=-1*B_K1[2];    B_K1[1][0]=B_K1[2][3]=B_K1[0];    B_K1[2][0]=B_K1[3][1]=B_K1[1]; B_K1[3][0]=B_K1[1][2]=B_K1[2];          Assigning a value of B_k1 to (i=0;i<4;i++) {for (j=0;j<4;j++) {  temp_44_1[i][j]=0;            temp_44_2[i][j]=0;        temp_44_3[i][j]=0;    }} temp_44_1[0][0]=temp_44_1[1][1]=temp_44_1[2][2]=temp_44_1[3][3]=tr;    Matrixminus ((float*) Temp_44_1, (float*) M_k1_k, (float*) temp_44_2,4,4);    Matrixtrans ((float*) B_k1, (float*) temp_44_1,4,4);    Matrixmul ((float*) B_k1, (float*) M_k1_k, (float*) temp_44_3,4,4,4);    Matrixmul ((float*) Temp_44_3, (float*) Temp_44_1, (float*) temp_44_4,4,4,4);    Matrixminus ((float*) Temp_44_2, (float*) Temp_44_4, (float*) temp_44_1,4,4);        for (i=0;i<4;i++) {for (j=0;j<4;j++) {p_k1_k_v[i][j]=0.25*rou_k*temp_44_1[i][j]; }}//Calculate p_k1_k_v for (i=0;i<4;i++) {fo            R (j=0;j<4;j++) {temp_44_1[i][j]=0;            temp_44_2[i][j]=0;        temp_44_3[i][j]=0;    }} Matrixtrans ((float*) H_k1, (float*) temp_44_4,4,4); Matrixmul (float*) H_k1, (float*) p_k1_k_q, (float*) temp_44_1,4,4, 4);    Matrixmul ((float*) Temp_44_1, (float*) Temp_44_4, (float*) temp_44_2,4,4,4);   Matrixadd ((float*) Temp_44_2, (float*) P_k1_k_v, (float*) s_k1_k,4,4);    Calculation S_k1_k Matrixtrans ((float*) h_k1_2, (float*) temp_74,4,7);    Matrixmul ((float*) P_k1_k, (float*) temp_74, (float*) temp_74_2,7,7,4);        for (i=0;i<4;i++) {for (j=0;j<4;j++) {temp_44_1[i][j]=s_k1_k[i][j];    }} Gauss_jordan ((float*) temp_44_1,4);      Matrixmul ((float*) Temp_74_2, (float*) Temp_44_1, (float*) k_k1,7,4,4);    Calculation K_k1 Matrixmul ((float*) K_k1, (float*) h_k1_2, (float*) temp_77_3,7,4,7);    Matrixminus ((float*) I7, (float*) Temp_77_3, (float*) temp_77_1,7,7);    Matrixtrans ((float*) Temp_77_1, (float*) temp_77_2,7,7);    Matrixmul ((float*) Temp_77_1, (float*) P_k1_k, (float*) temp_77_3,7,7,7);     Matrixmul ((float*) Temp_77_3, (float*) temp_77_2, (float*) temp_77_4,7,7,7);         Matrixmul ((float*) K_k1, (float*) P_k1_k_v, (float*) temp_74,7,4,4);    Matrixtrans ((float*) K_k1, (float*) temp_47_1,7,4); MatrixmuL ((float*) temp_74, (float*) Temp_47_1, (float*) temp_77_3,7,4,7);   Matrixadd ((float*) Temp_77_4, (float*) Temp_77_3, (float*) p_k1_k1,7,7);        Calculates P_K1_K1 for (i=0;i<7;i++) {for (j=0;j<7;j++) {p_k_k[i][j]=p_k1_k1[i][j]; }}//p_k1_k1 is saved as the next p_k_k for (i=0;i<4;i++) {for (j=0;j<4        ; j + +) {P_k_k_q[i][j]=p_k1_k1[i][j]; }}//extract from P_k1_k1 p_k_k_q Matrixmul ((float*) Temp_77_1, (float*) X_k1_k, (Floa   t*) x_k1_k1,7,7,1);    Calculate x_k1_k1 for (i=0;i<3;i++) {u_k_k[i]=x_k1_k1[i+4];        }//Extract u_k_k for (i=0;i<4;i++) from X_K1_K1 {    Q_k1_k1[i]=x_k1_k1[i];    } temp=pow (q_k1_k1[0]*q_k1_k1[0]+q_k1_k1[1]*q_k1_k1[1]+q_k1_k1[2]*q_k1_k1[2]+q_k1_k1[3]*q_k1_k1[3],0.5);    for (i=0;i<4;i++) {q_k_k[i]=q_k1_k1[i]/temp;                      }                                              Extract four yuan from X_K1_K1 and save it for the next four-dollar number} 

Reference documents

[1] Kanji, Lu Jinfu. Fundamentals of Numerical analysis [M]. Tongji University Press. 1998:75-80.

[2] Kanji, Lu Jinfu. Fundamentals of Numerical analysis [M]. Tongji University Press. 1998:38-41.

[3] Kanji, Lu Jinfu. Fundamentals of Numerical analysis [M]. Tongji University Press. 1998:59-60.

[4] Yang Piwen. Four meta-number analysis and partial differential equations [M]. Science Press. 2009.

[5] Qin Yongyuan, Wang Shuhua. Kalman filter and combined navigation Principle (second edition) [M]. Northwestern Polytechnical University Press. 2012.

[6] Dung Zhenglong. Inertial technology [M]. Northwestern Polytechnical University Press. 2006.

[7] Zhang Skylight, contacts. Strapdown Inertial navigation technology [M]. National defense industry Press. 2007.

Kalman Filter (vi)--Application of Kalman filter: four-element Kalman filter (QKF) C code to realize attitude calculation