Kalman Filter 简单理解1

一个线性时变系统，可以以下面的方程表示：

$\mathbf{x_t} = \mathbf{F_tx_{t-1}}+\mathbf{B_tu_t}+\mathbf{w_t}$

同时该系统可以被观测：

$\mathbf{z_t} = \mathbf{H_tx_t} + \mathbf{v_t}$

其中， $\mathbf{w_t}$ 和 $\mathbf{v_t}$ 分别是状态传递噪声和观测噪声。一般假定服从均值为0的高斯分布，协方差矩阵分别以 $\mathbf{Q_t}$ 和 $\mathbf{R_t}$ 表示。

由于系统的真实状态不能直接获得，KF算法通过将状态转移模型和观测模型结合起来，提供了对系统状态的不断更新。预测阶段的公式如下：

$\mathbf{\tilde{x}_{t|t-1}} = \mathbf{F_t\tilde{x}_{t-1|t-1}}+\mathbf{B_tu_t}$

上式仅仅对系统的均值进行了更新，为了准确的描述系统的状态分布，需要利用系统状态参数的协方差矩阵，通常用 $\mathbf{P_{t|t-1}}$ 表示。 $\mathbf{P_{t|t-1}}$ 可以通过下式求得：

$\mathbf{P_{t|t-1}} = E[(x_t-\hat{x}_{t|t-1})(x_t-\hat{x}_{t|t-1})^T]$

利用(3)式，可以求出：

$\mathbf{P_{t|t-1}} = \mathbf{F_t}\mathbf{P_{t-1|t-1}}\mathbf{F_t}^T + \mathbf{Q_t}$

利用观测方程更新状态方程可以写成如下公式，先不必纠结怎么得到：

$\mathbf{\tilde{x}_{t|t}} = \mathbf{\hat{x}_{t|t-1}} + \mathbf{K_t(z_t-H_t\hat{x}_{t|t-1})}$ $\mathbf{P_{t|t-1}} = \mathbf{P_{t|t-1}}-\mathbf{K_tH_tP_{t|t-1}}$

其中，

$\mathbf{K_t} = \mathbf{P_{t|t-1}H_t^T(H_tP_{t|t-1}H_t^T+R_t)^{-1}}$

为了理解(5)-(8)式，可以通过一个简单的例子来直观的说明其正确性。

上图中，一个火车沿铁路前进，根据上一个状态，以及油门，可以估计当前的位置，如图中粉色高斯函数。而通过无线电设备，同样可以观测火车当前的位置，如图中蓝色的区域。为了更精确的描述火车的位置，我们将两个高斯函数相乘，得到最终的状态预测结果，如图中绿色的区域。

更详细的，预测的概率密度函数服从![](http://latex.codecogs.com/gif.latex?y_1 \sim N(\mu_1, \sigma_1^2))，观测的概率密度函数服从![](http://latex.codecogs.com/gif.latex?y_2 \sim N(\mu_2, \sigma_2^2))。根据高斯函数相乘的性质，可以得到，

$y_{fused} \sim N(\mu_{fused}, \sigma_{fused}^2)\tag{9}$

其中，

$\mu_{fused} = \frac{\mu_1\sigma_2^2+\mu_2\sigma_1^2}{\sigma_1^2+\sigma_2^2} = \mu_1+\frac{\sigma_1^2(\mu_2-\mu_1)}{\sigma_1^2+\sigma_2^2} = \mu_1+K(\mu_2-H\mu_1)$ $\sigma_{fused}^2 = \frac{\sigma_1^2\sigma_2^2}{\sigma_1^2+\sigma_2^2}=\sigma_1^2-\frac{\mu_1^4}{\sigma_1^2+\sigma_2^2} = \mu_1^2-KH\mu_1^2$

其中， $K$ 为卡尔曼增益，而 $H$ 为转移矩阵，描述了观测方程和状态方程的映射关系。

参考文献

https://synapticlab.co.kr/attachment/cfile1.uf@2737C54B590907BA0D46CE.pdf