从矩阵运算理解KF

初学卡尔曼滤波卡尔曼学完可能会发现好简单啊就是权重的概念，对，没错。但是用它来平滑原数据进行预测，引入高维卡尔曼滤波绝非易事。

x状态矩阵

A即是F 状态转移矩阵

B控制矩阵

u控制向量

P协方差矩阵

Q预测误差

R观测误差

H 转换矩阵

z_观测矩阵

z预测的矩阵

第一步

x = A * x + B * U;也就是X=X0+V0*T+1/2aTe2

 

 

 

第二步P = A * P * A_T + Q;

 

 

第三步 z = H * x;

 

 

这两个值就是二维预测结果

第四步 o1 = H*P*HT + R_;

 

 

第五步Bulf2 = Bulf1.inverse();

矩阵求逆

 

 

第六步 K = P*HT*Bulf2

 

 

P = P - K*H*P

输入观测值z_

x = x + K*(z_ - z);

自己计算一遍，效果最佳！