卡尔曼滤波公式

线性系统模型

状态转移方程:

\[x_{k}=F_{k}x_{k-1}+B_{k}u_{k}+n_{k} \]

传感器观测模型:

\[z_{k}=H_{k}x_{k}+v_{k} \]

其中\(x_{k}\)是状态变量,\(z_{k}\)是带有噪声的观测量,\(F_{k}\)是为系统矩阵,\(H_{k}\)是观测矩阵。\(n_{k}\)是系统噪声,服从均值为\(0\),协方差为\(Q_{k}\)的高斯分布;\(v_{k}\)是测量噪声,服从均值为\(0\),协方差为\(R_{k}\)的高斯分布。

KF滤波公式

假设已经知道\(x_{k-1}\)服从均值为\(\mu _{k-1}\),方差为\(P_{k-1}\)的高斯分布。那么由状态转移模型可以得到对\(x_{k}\)服从的高斯分布:

\[x^{-}_{k}=F_{k}\hat x_{k-1}+B_{k}u_{k} \]

\[P_{k}^{-}=F_{k}P_{k-1}F_{k}^{T}+Q_{k} \]

这里没有参考第\(k\)次测量。如果加上第\(k\)次观测,可以得到\(x_{k}\)的分布,其均值与方差为:

\[\hat x_{k}=\hat x_{k}^{-}+K\left(z_{k}-Hx_{k}^{-}\right) \]

\[P_{k}=P_{k}^{-}-KH_{k}P_{k}^{-} \]

\[K=P_{k}^{-}H_{k}^{T}\left(H_{k}P_{k}^{-}H_{k}^{T}+R_{k}\right)^{-1} \]

posted @ 2020-03-25 15:24  心田居士  阅读(1096)  评论(0编辑  收藏  举报