卡尔曼滤波-C语言 卡尔曼滤波学习(7)http://t.csdnimg.cn/4ubGO

#include "kalman.h" /****************************************************** 先验估计值：X(k|k-1) = AX(k-1|k-1) + BU(k-1|k-1) 误差协方差的先验估计：P(k|k-1) = AP(k-1|k-1)A^T + Q 卡尔曼增益：K(k) = (P(k|k-1)H^T)/(HP(k|k-1)H^T + R) 后验估计值：X(k|k) = X(k|k-1) + K(k)*(Z(k) - HX(k|k-1)) 更新误差协方差：P(k|k) = (I - K(k)H)P(k|k-1) ******************************************************/ /****************************************************** 设小车初始行程x_1 = 0m，速度x_2 = 1m/s，匀速运动 现在由GPS测量小车的行程z_1，速度z_2 单位时间deltaT= 1 所有误差符合正态分布 A = 1, 1 B = 0 0, 1 0 Q = 0.1, 0.0 R = 1.0, 0.0 0.0, 0.1 0.0, 1.0 H = 1,0 X_0 = 0 P_k = 1, 0 0,1 0 0, 1 ******************************************************/ #define A_ROWS 2 #define A_COLS 2 #define A_LENGTH (A_ROWS*A_COLS) #define B_ROWS 2 #define B_COLS 1 #define B_LENGTH (B_ROWS*B_COLS) #define Q_ROWS 2 #define Q_COLS 2 #define Q_LENGTH (Q_ROWS*Q_COLS) #define R_ROWS 2 #define R_COLS 2 #define R_LENGTH (R_ROWS*R_COLS) #define H_ROWS 2 #define H_COLS 2 #define H_LENGTH (H_ROWS*H_COLS) #define P_ROWS 2 #define P_COLS 2 #define P_LENGTH (P_ROWS*P_COLS) #define X_ROWS 2 #define X_COLS 1 #define X_LENGTH (X_ROWS*X_COLS) #define K_ROWS 2 #define K_COLS 2 #define K_LENGTH (K_ROWS*K_COLS) #define Z_ROWS 2 #define Z_COLS 1 #define Z_LENGTH (Z_ROWS*Z_COLS) #define I_ROWS 2 #define I_COLS 2 #define I_LENGTH (I_ROWS*I_COLS) #define X_OPT_0 {0.0,0.0} #define P_OPT_0 {1.0, 0.0, 0.0, 1.0} double A_mat[A_LENGTH] = {1.0, 1.0, 0.0, 1.0}; double B_mat[B_LENGTH] = {0.0, 0.0}; double Q_mat[Q_LENGTH] = {0.1, 0.0, 0.0, 0.1}; double R_mat[R_LENGTH] = {1, 0.0, 0.0, 1}; double H_mat[H_LENGTH] = {1.0, 0.0, 0.0, 1.0}; double K_mat[K_LENGTH] = {0.0, 0.0, 0.0, 0.0}; double Z_mat[Z_LENGTH] = {0.0, 0.0}; double U[1] = {0}; double I_mat[I_LENGTH] = {1.0, 0.0, 0.0, 1.0}; typedef struct{ double P_Prior[P_LENGTH]; double P_Optimal[P_LENGTH]; double X_Prior[X_LENGTH]; //先验值 double X_Optimal[X_LENGTH]; //最优值 }kalman_value; kalman_value Kalman_Opt = {.P_Prior = {0.0,0.0,0.0,0.0}, .P_Optimal = P_OPT_0, .X_Prior = {0.0, 0.0}, .X_Optimal = X_OPT_0}; void Kalman_Init(void) { for(int i = 0; i<X_LENGTH; i++) { Kalman_Opt.X_Prior[i] = 0.0; } for(int i = 0; i<P_LENGTH; i++) { Kalman_Opt.P_Prior[i] = 0.0; } } void Kalman_Calculate(double * measure_value) { double A_X_Optimal_mat[X_LENGTH]; double B_U_mat[B_LENGTH] = {0.0,0.0}; double A_T_mat[P_LENGTH]; double A_P_mat[P_LENGTH]; double A_P_A_T_mat[P_LENGTH]; double A_P_A_T_Q_mat[P_LENGTH]; double H_T_mat[H_LENGTH]; double P_H_T_mat[P_LENGTH]; double H_P_mat[P_LENGTH]; double HP_HT_mat[P_LENGTH]; double HPHT_R_mat[P_LENGTH]; double HPHTR_mat[P_LENGTH]; double H_X_mat[X_LENGTH]; double Z_HX_mat[Z_LENGTH]; double K_ZHX_mat[Z_LENGTH]; double K_H_mat[K_LENGTH]; double I_KH_mat[K_LENGTH]; //X的先验估计值 matrix_multiplication(A_mat, Kalman_Opt.X_Optimal, A_X_Optimal_mat, A_ROWS, A_COLS, X_COLS); //A*X // matrix_multiplication((double *)B_mat, (double *)U, B_U_mat, B_ROWS, B_COLS, 1); //B*U matrix_addition(A_X_Optimal_mat, B_U_mat, Kalman_Opt.X_Prior, X_ROWS, X_COLS); //X(k|k-1) = AX(k-1|k-1) + BU(k-1|k-1) //P的先验 matrix_multiplication(A_mat, Kalman_Opt.P_Optimal, A_P_mat, A_ROWS, A_COLS, P_COLS); matrix_transpose(A_mat, A_T_mat, A_ROWS, A_COLS); matrix_multiplication(A_P_mat, A_T_mat, A_P_A_T_mat, A_ROWS, A_COLS, A_COLS); matrix_addition(A_P_A_T_mat, Q_mat, Kalman_Opt.P_Prior, A_ROWS, A_COLS); //P(k|k-1) = AP(k|k-1)A^T + Q //K卡尔曼增益 matrix_transpose(H_mat, H_T_mat, H_ROWS, H_COLS); matrix_multiplication(Kalman_Opt.P_Optimal, H_T_mat, P_H_T_mat, P_ROWS, P_COLS, H_COLS); matrix_multiplication(H_mat, Kalman_Opt.P_Optimal, H_P_mat, H_ROWS, H_COLS, P_COLS); matrix_multiplication(H_P_mat, H_T_mat, HP_HT_mat, P_ROWS, P_COLS, H_COLS); matrix_addition(HP_HT_mat, R_mat, HPHT_R_mat, P_ROWS,P_COLS); matrix_inverse(HPHT_R_mat, HPHTR_mat, P_ROWS); matrix_multiplication(P_H_T_mat, HPHTR_mat, K_mat, P_ROWS, P_COLS, P_COLS);//K(k) = (P(k|k-1)H^T)/(HP(k|k-1)H^T + R) //X的最优估计值 matrix_multiplication(H_mat, Kalman_Opt.X_Prior, H_X_mat, H_ROWS, H_COLS, X_COLS); matrix_subtraction(measure_value, H_X_mat, Z_HX_mat, Z_ROWS, Z_COLS); matrix_multiplication(K_mat, Z_HX_mat, K_ZHX_mat, K_ROWS, K_COLS, X_COLS); matrix_addition(Kalman_Opt.X_Prior, K_ZHX_mat, Kalman_Opt.X_Optimal, X_ROWS, X_COLS); //X(k|k) = X(k|k-1) + K(k)*(Z(k) - HX(k|k-1)) //P矩阵更新 P(k|k) = (I - K(k)H)P(k|k-1) matrix_multiplication(K_mat, H_mat, K_H_mat, K_ROWS, K_COLS, H_COLS); matrix_subtraction(I_mat, K_H_mat, I_KH_mat, K_ROWS, K_COLS); matrix_multiplication(I_KH_mat, Kalman_Opt.P_Prior, Kalman_Opt.P_Optimal, K_ROWS, K_COLS, P_COLS); } /**************************************************************卡尔曼滤波测试*********************************************************************/ double X_test_data[400] = {0.0,1.0}; //x[i*2]行程， x[i*2+1]速度 模拟真实值 double Z_test_data[400] = {0.0,0.0}; //z[i*2]行程， z[i*2+1]速度 模拟测量值 //X_error_vlaue模拟真实值中的误差，符合正态分布 //Z_error_vlaue模拟测量值中的误差，符合正态分布 double X_error_vlaue[400] = {-0.163043,0.049034,0.175279,0.037038,0.477885,0.201506,0.328701,-0.366056,-0.093273,-0.528352, 0.233015,-0.287299,0.086006,-0.046547,-0.179422,0.268040,0.152329,-0.156242,0.426203,0.111814, -0.012498,0.301227,0.528005,-0.002734,0.585343,0.005500,0.095425,-0.054205,-0.566146,0.435157, -0.298545,0.348202,-0.120855,0.398873,0.900686,-0.517118,-0.058621,-0.418217,0.109107,-0.401504, -0.186339,-0.222289,0.109535,-0.046732,0.054043,0.048720,0.301145,0.493835,0.287985,-0.595013, -0.043576,0.064509,0.439793,-0.187932,0.154235,0.355500,-0.517815,0.374972,-0.195772,-0.174628, -0.068504,-0.478056,-0.311314,0.462194,0.237629,0.393914,-0.254160,-0.136892,-0.440900,0.030102, -0.310817,0.059631,0.120912,1.102323,-0.024978,0.083417,-0.151538,-0.087603,0.387968,-0.201677, 0.091967,-0.445459,-0.493259,-0.238241,-0.051796,0.364439,-0.493450,0.277612,-0.410607,0.339569, 0.349924,0.177913,0.490797,-0.467658,-0.589160,0.158132,0.241536,0.010984,0.458754,0.325478, -0.538413,-0.254849,-0.338034,0.228464,-0.953588,-0.364181,0.252785,-0.110609,0.021102,-0.163898, 0.331535,0.616888,0.336602,0.540951,0.436471,-0.407511,0.023843,0.063591,-0.135623,-0.029515, 0.511256,-0.068142,0.276152,-0.304820,-0.067581,-0.001160,0.341417,0.181825,-0.092443,-0.222536, -0.112368,-0.467460,-0.380314,-0.278527,0.036326,-0.510108,-0.364577,-0.190006,0.394168,0.001285, -0.197726,-0.042118,0.054749,0.044044,-0.094533,0.693770,-0.562106,-0.335974,0.814059,0.151571, 0.181314,0.025654,0.060893,0.034066,-0.117167,-0.647020,-0.553737,-0.847473,0.057900,0.532570, 0.358863,-0.068955,0.043818,-0.102883,-0.092258,-0.081661,-0.322829,0.410192,0.410727,0.606394, 0.035576,-0.105524,0.604857,0.681441,-0.306380,-0.195051,-0.072543,0.128497,0.155929,0.240502, -0.363598,-0.046440,0.765299,0.145683,-0.576400,-0.063420,-0.479671,-0.069416,0.080994,-0.206915, -0.378033,-0.307537,-0.531580,-0.089880,0.460555,-0.142611,0.316901,0.116487,-0.244764,-0.412239, 0.219388,0.100134,0.242546,-0.728656,0.024279,0.467224,0.063906,0.070350,-0.499968,0.103796, 0.042035,0.274390,0.082419,-0.206692,-0.299192,0.362526,0.246523,0.022421,-0.272247,0.656083, 0.194871,-0.362098,-0.462558,-0.142794,0.583367,-0.246378,0.049982,-0.088080,0.057668,-0.436670, 0.251793,-0.551453,0.474589,-0.014656,-0.727778,-0.667444,0.543782,-0.256944,0.396085,-0.268196, -0.025300,-0.098