卡尔曼滤波公式

线性系统模型

状态转移方程：

\[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} \]