卡尔曼滤波器 Kalman Filter *

卡尔曼滤波适用于 线性高斯系统，即该系统必须是线性的，而且噪声服从正态分布。 更详细一些，噪声通常被建模为一个均值为 0、方差为常数的正态分布，也就是高斯分布。该正态分布的横坐标是随机变量的取值，纵坐标是对应取值的概率密度。

带有过程噪声的线性离散系统的状态空间方程

观测向量为

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

先验状态估计

\[\widehat{x}_{[k]}^- = A \widehat{x}_{[k-1]} + B u_{[k-1]}\]

先验矩阵

\[P_{[k]}^- = A P_{[k]} A^T + Q_c\]

卡尔曼增益

\[K_{[k]} = P_{[k]}^- H_m^T (H_m P_{[k]}^- H_m^T + R_c)^{-1}\]

计算后验状态估计：

\[\widehat{x}_{[k]} = \widehat{x}_{[k]}^- + K_{[k]} (z_{[k]} - H_m \widehat{x}_{[k]}^-)\]

后验状态估计误差协方差矩阵:

\[P_{[k]} = (I - K_{[k]} H_m)P_{[k]}^-\]

请参阅 协方差 了解更多信息。