卡尔曼滤波器_Recursive Processing

【卡尔曼滤波器】1_递归算法_Recursive Processing

文章是根据上面视频进行个人总结

 得到公式

当前估计值 = 上次估计值 + 系数（当前测量值-上次估计值）

性质

K越大，Xk趋近于Xk-1,测量结果影响越小

1/K=系数=Kk     估计误差 e(est)   测量误差 e(mea)

Kk =  e(est)K / ( e(est)K-1 + e(mea)K )

性质

e(estK-1) >> e(meaK)   Kk趋近1 相信当前测量值

e(estK-1) << e(meaK)   Kk趋近0 相信上一次测量值

有三条重要公式

Step2:Kk =  e(est)K / ( e(est)K-1 + e(mea)K )

Step2:Xk = Xk-1 + Kk * (Zk-Xk-1)

Step3:e(estK) = (1-Kk) * e(estK-1)

使用测量硬币宽度举例子

实际长度 X=50mm  估计值   X0=40mm  估计误差 e(est0) = 5mm 测量值 Z1=51mm 测量误差 eMea=3mm

第一次测量值为 51mm 第二次测量值为48mm 

通过上述公式算出第二行的值

Kk=5/(5+3)=0.625   Xk= 40 + 5/8(51-40) = 46.875  e(est)=(1-5/8)*5=1.875

后面类推可以得到这样数值与图像（蓝色为Zk，红色Xk） 

结果：

Xk估计值越来越接近50

Zk测量值分布在50左右，误差不超过正负3

实现了简单的卡尔曼滤波器，递归算法