#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
