卡尔曼滤波建立在隐马尔科夫模型上，是一种递归估计。也就是说，只需要知道上一个状态的估计值，以及当前状态的观测值，就能计算当前状态的最优估计值。

而不需要更早的历史信息。

卡尔曼滤波器的2个状态

1.最优估计

2.误差协方差矩阵

这两个变量迭代计算，初始值多少，其实没有影响。反正最后都能收敛到最优估计。

预测过程

F是状态转移矩阵，B是控制矩阵（也可以不需要）。Q是过程噪声的协方差。

这里等式左边小勾勾表示估计量，有个负号表示，这个估计量还不优，差点东西。

更新过程

第1个式子，是状态更新过程。H是测量矩阵，Z是观察矩阵，括号里是测量残差。

第2个式子，是卡尔曼增益矩阵。R是观察噪声的协方差矩阵。

第3个式子，是误差协方差矩阵更新过程。

于是可以开始迭代了。我们以小汽车的[位置速度]为状态变量，小车做匀速运动。

Z=(1:100)+0.1*randn(1,100);%观测值加方差为1的白噪声
X=[0.8;1.2];%初始最优估计状态
P=[1.2 0.9;0.8 1.3];%初始最优协方差矩阵
F=[1 1;0 1];%状态转移矩阵
Q=[0.001 0;0 0.001];%预测噪声协方差矩阵
H=[1 0];%观测矩阵
R=1;%观测噪声协方差矩阵
hold on
for i=1:100
    X_=F*X;%这里没有控制量
    P_=F*P*F'+Q;
    K=P_*H'/(H*P_*H'+R);
    X=X_+K*(Z(i)-H*X_);
    P=(eye(2)-K*H)*P_;
    plot(X(1),X(2),'*');    
end

可以看到，虽然初始状态随便写，但是很快就收敛到了真实值附近。预测噪声协方差矩阵Q要小一点才行，这表示我们对状态转移矩阵的信心足够大。否则预测效果会很差。

欢迎参与讨论并关注本博客和微博以及知乎个人主页后续内容继续更新哦~

转载请您尊重作者的劳动，完整保留上述文字以及文章链接，谢谢您的支持！