【多目标跟踪学习笔记】1.卡尔曼滤波(Kalman Filter)

零 前言
在有些场景中，我们希望通过来自不同方面的数据来预测系统的下一个状态。卡尔曼滤波是解决此类问题的一个算法，但是其只能应用于线性的高斯系统。
  \space  
  \space  
一 引言
通过一个简单的例子先来说明。假如我们想预测一辆车下一时刻的位置，我们有两大数据来源：一是IMU(Internal Measurement Unit,可以测量加速度、角速度等)，二是GPS，三是速度表。结合这三个测量值，我们可以估算出汽车的位置。卡尔曼滤波就可以应用于这个场景。

卡尔曼滤波总体的两大用处：

• 可以优化估算一些无法直接测量但可以间接测量的量
• 可以结合多个传感器的信息来估测系统现在的状态
    \space  
    \space  

二 从状态观测器的角度理解卡尔曼滤波
先写一下控制理论里面的几个简单概念：

• 状态变量： 确定系统状态的一组独立(最小)变量。是能完整、确定地描述系统的时域行为的最少的一组变量。
• 状态方程： 描述系统的状态变量之间，及其和系统输入量之间关系的一阶微分方程组。
• 输出方程： 描述系统输出变量与状态变量（有时还包括输入变量）之间的函数关系的代数方程。
• 状态观测器： 为了实现状态反馈控制，就要设法利用巳知的信息（输入量及输出量），通过一个模型来对状态变量进行估计。

系统的状态空间表达式由状态方程和输出方程组成，一般可以表示为：
$$\begin{gathered} \dot{x}=Ax+Bu \\ y=Cx \end{gathered}$$
我们设计一个状态观测器 (在问题中，就是我们建立的模型) 来估计系统状态，状态观测器的系统状态变量和输出都用字母加hat表示，其状态空间表达式为：
$$\begin{gathered} \dot{\hat{x}}=A\hat{x}+Bu \\ \hat{y}=C\hat{x} \end{gathered}$$
系统框图如下：

蓝色部分就是state observer，K是负反馈系统，用于将误差$x-\hat{x}$最小化。

误差的表达式为：
$$e_{obs}=x-\hat{x}$$
由：
$$\begin{gathered} \dot{x}=Ax+Bu \\ \dot{\hat{x}}=A\hat{x}+Bu+K(y-\hat{y}) \end{gathered}$$
得到:
$$\dot{e_{obs}}=\dot{x}-\dot{\hat{x}}=A(x-\hat{x})-K(C(x-\hat{x}))=A{e}_{obs}-KC{e}_{obs}=(A-KC)e_{obs}$$
解这个微分方程，得到:
$$e_{obs}(t)=e^{(A-KC)t}{e}_{obs}(0)$$
所以，如果$A-KC<0$,则误差会收敛，否则不会。 因此$K$的选择对于收敛速度就尤为重要，选择$K$的方法，就是使用Kalman滤波器。

  \space  
  \space  
三 卡尔曼滤波的工作原理
先以一个最简单的一维例子说明。假设我们要造一个自动驾驶汽车，而且想要时刻预测它的位置。那么状态向量就只有位置信息一个，即$k$时刻的状态为：
$$x_k=[position]$$
我们要获取的信息就是小车的位置，因此
$$y_k=C{x}_k=x_k$$
假设系统的输入是速度，考虑噪声(干扰)，其框图可表示如下：

其中$A$为状态转换矩阵，将上一状态投射到这一状态；$B$为控制输入矩阵，将输入投射到当前状态；$v_k\sim N(0,R),w_k\sim N(0,Q)$.(卡尔曼滤波适用于线性、高斯)
这时候，跟之前说的模型一样，我们有一个自己计算小车位置的模型。此时卡尔曼滤波的作用就是将我们模型的预测值和测量值结合起来，得到最佳的新的状态的估计。 如下图所示：

• 好像跟前面第二部分$K$的作用有些出入，不过Kalman的确是“作用”在真实测量和模型预测之间，来减少误差的部分。所以Kalman滤波的确像一个状态观测器，只不过它是针对随机系统的。

具体怎么做呢？假设$k-1$时刻小车的状态为$x_{k-1}$，其是一个高斯分布。第$k$时刻状态为$x_k$，且方差会增加(为什么？)，测量值也是高斯分布，我们将这两个高斯分布相乘，就能得到最佳估计值，如下图所示：

这就是一维case的基本思想。

我手推了一下(懒得打公式了)，思路参考的是卡尔曼滤波：从入门到精通，知乎专栏

  \space     \space  
四 将一维情形推广至高维情形

因此，卡尔曼滤波就是一个将预测和测量结合，进行数据融合的算法，如此进行迭代下去，就可以预测每一时刻的状态。

再总结一下，Kalman滤波分两步，
1.predict:
$$\begin{gathered} {\widehat{x_t}}^{-}=A\widehat{x_{t-1}}+B{u}_t \\ {P}_t^{-}=A{P}_{t-1}{A}^T+Q \end{gathered}$$
2.update:
$$\begin{gathered} K_t=\frac{C_t{P}_t^{-}}{C_t{P}_t^{-}{C}_t^T+R},称作卡尔曼增益 \\ \widehat{x_t}={\widehat{x_t}}^{-}+K_t(y_t-C_t{\widehat{x_t}}^{-}) \\ {P}_t=(I-K_t{C}_t)P_t^{-} \end{gathered}$$

可以看到，状态更新方程$\widehat{x_t}={\widehat{x_t}}^{-}+K_t(y_t-C_t{\widehat{x_t}}^{-})$很像状态观测器方程，再次说明了卡尔曼滤波就是一种特殊的state observer。

  \space  
注
卡尔曼滤波只适用于线性高斯分布情形，对于其他情形有扩展卡尔曼滤波、粒子滤波等。以后再学，把他们的特点和使用条件先记一下。