公式来自于:https://blog.csdn.net/u010720661/article/details/63253509

左上角图来自于:https://blog.csdn.net/AdamShan/article/details/78248421

注意的地方:kalman是将预测经过尺度变换H后,再与测量值进行加权。

标准kalman过程

根据 大名鼎鼎的跟踪算法Sort源码[https://github.com/abewley/sort]采用的kalman库【...\Lib\site-packages\filterpy\kalman\kalman_filter.py】中,kalman计算过程如下:

# = = = 整个计算可以浓缩为 = = = = = = 
    输入测量值z
    输入上次最优值x,若初始,则保存z到x中
    x = Fx + Bu
    P = F*P*F' + Q
# 有测量值z时:
    y=z-H*x
    S = H*P*H' + R
    K = P*H'*inv(S)
   x = x + Ky
   P = (I-KH)P(I-KH)' + KRK'
# 无测量值z时:
   x=x
   P=P 
# = = = 备注 = = = = = = = = = = = = 
# 更新P时,P = (I-KH)P(I-KH)' + KRK'比P = (I-KH)P更稳定
# 转移矩阵 H*x  <->  z

相关文章: