卡尔曼滤波器

之前一直认为卡尔曼滤波器是很难的一个存在，尤其是当看到很多文章中介绍的时候都是从数学入手，搞的以前见到就觉得头大。

今天我们就从一个简单的例子来介绍，一个物体做直线匀加速运动，其数学模型为：

$d_{k} = d_{k-1} + v_{k-1}t + \frac{1}{2}at^2$ $v_{k} = v_{k-1} + at$

实际上着是一个典型的离散控制系统，当我们设置状态向量$X$ 为 $ \begin{bmatrix} d
v \end{bmatrix} $ 这个系统可以简单的表示为

$X_{k} = AX_{k - 1} + Bu$

我们如果只关心位置的话，系统的输出方程为

$y = CX$

此时系统方程中 $A = \begin{bmatrix} 1 & t \ 0 & 1 \end{bmatrix}$, $B = \begin{bmatrix} \frac{1}{2}t^2 \ t \end{bmatrix}$, $C = \begin{bmatrix} 1 & 0 \end{bmatrix}$.

假设我们控制向量$u$ 存在一个误差，该误差满足正态分布$[\bar{u}, \sigma_{u}]$，我们在系统中有一个sensor实时的测量移动距离，但是该sensor也存在测量误差，且满足正态分布$[\bar{m}, \sigma_{m}]$, 方差 $R = \sigma _{m}^{2}$ 。

下边根据上边已知的数据来计算给该系统设计估计器，估计器的表达式就是系统的状态方程，但是系统有噪声，我们要考虑噪声因素。由于我们控制向量$u$的标准方差为$\sigma_{u}$，那么 $Bu$ 的协方差矩阵为$Q = \begin{bmatrix}\frac{1}{4}t^{4} & \frac{1}{2}t^{3} \ \frac{1}{2}t^{3} & t^{2}\end{bmatrix}$, $P_{0} = Q$

这样我们的估计器的表达式为

$X_{k} = AX_{k-1} + Bu \\ P_{k} = AP_{k-1}A^{T} + Q$

而卡尔曼滤波器的精髓是所谓的卡尔曼增益，用卡尔曼增益乘以估计值和测量值的结果，可以更新估计值，在控制系统中这叫状态反馈。

$K = P_{k}C^{T} + (CP_{k}C^{T} + R)^{-1}$ $\hat{X_{k}} = X_{k} + K(Z - CX_{k})$ $\hat{P_{k}} = P_{k} - KCP_{k}$

然后再来一轮，一直循环下去。

关于kalman滤波器的实现和测试代码请参考我的github中实现代码，测试代码.

测试代码执行完后我们可以看到以下结果