Kalman filter算法介绍及Python实现

一、算法思路

1.1 Kalman filter简介

卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

数据滤波是去除噪声还原真实数据的一种数据处理技术，Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态。由于它便于计算机编程实现，并能够对现场采集的数据进行实时的更新和处理，Kalman滤波是目前应用最为广泛的滤波方法，在通信，导航，制导与控制等多领域得到了较好的应用。

1.2 算法推导

由于卡尔曼滤波是建立在三种状态下的预测模型，即现在我们现定义三种状态：

1. 现实状态(1)，也就是物体真实的轨迹
2. 测量状态(2)，也就是传感器返回的数值
3. 预测状态(3)，根据能动方程推论状态
   $$\{\begin{array}{l}{X}_t=F_t{X}_t+B_t{U}_t+{\omega }_t(1)\\ Z_t=H_t{X}_t+V_t(2)\\ {\hat{X}}_t=F_t{\hat{X}}_{t-1}+B_t+{\mu }_t(3)\end{array}$$

其中(1)(2)，大概就是位移随着时间的变化具有一定的线性关系，$f(x),h(x)$就是这种抽象关系，可以故且看作一个基于运动方程的线性表出：
$$\{\begin{array}{l}{X}_t=f(x_{t-1},{\mu }_t)+{\omega }_t\\ Z_{t,j}=h(y_j,x_t)+v_{t,j}\end{array}$$

• $x_t,y_j$为$t$时刻的状态向量，包括了相机位姿、路标坐标等信息，也可能有速度、朝向等信息（方便后续推断，将$y_j$路标并入到状态里）；
• ${\mu }_t$ 为运动测量值，如加速度，转向等等；
• $F_t$ 为状态转换方程，将$t-1$时刻的状态转换至$t$时刻的状态；
• $B_t$ 是控制输入矩阵，将运动测量值​${\mu }_t$的作用映射到状态向量上；
• ${\omega }_t$是预测的高斯噪声，其均值为0，协方差矩阵为$Q_t$。

其中(3)，用一个在解释卡尔曼滤波时最常用的一维例子『小车追踪』，如下图所示：

由初中物理可得：
$$\{\begin{array}{l}x=x_{t-1}+v_t\times \Delta t+\frac{1}{2}a\Delta t^2\\ v_t=v_{t-1}+at\end{array}$$
写成向量的形式就是：
$$\left[\begin{array}{c}{x}_t\\ v_t\end{array}\right]=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\ast \left[\begin{array}{c}{x}_{t-1}\\ v_{t-1}\end{array}\right]+\left[\begin{array}{c}\frac{1}{2}\Delta t^2\\ \Delta t\end{array}\right]\ast a$$
这时候你把公式等价化：
$$a\sim {\mu }_t$$$$\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\&\&\left[\begin{array}{c}\frac{1}{2}\Delta t^2\\ \Delta t\end{array}\right]\sim F_t,B_t$$
就会发现动力学的预测公式就是这么个东西$${\hat{X}}_{t|t-1}=F_t{\hat{X}}_{t-1}+B_t+{\mu }_t$$

• $F_t$ 为状态转换方程，将$t-1$时刻的状态转换至$t$时刻的状态；
• $B_t$ 是控制输入矩阵，将运动测量值​${\mu }_t$的作用映射到状态向量上；
• ${\hat{X}}_{t|t-1}$ 代表由$X_{t-1}$预测所来的$X_t$的值，为了和下面传感器的返回数值${\hat{X}}_t$区分就用这个来代替

用『式子(1)-(3)』：
$$X_t-{\hat{X}}_{t|t-1}=F_t(X_{t-1}-{\hat{X}}_{t-1})+{\omega }_t$$
于是:$$\begin{gathered} P_{t|t-1}=E[(X_t-{\hat{X}}_{t|t-1})(X_t-{\hat{X}}_{t|t-1}{)}^T] \\ \dots \\ =F_t{P}_{t-1}{F}_t^T+Q_t \end{gathered}$$
可以看到，经过预测更新，协方差矩阵P变大了。这是因为状态转换并不完美，而且运动测量值含有噪声，具有较大的不确定性。

预测更新实际上相当于“加法”：将当前状态转换到下一时刻（并增加一定不确定性），再把外界的干扰（运动测量值）叠加上去（又增加了一点不确定性）。
得到了测量值$\hat{X}$，那么我们就可以对状态向量进行测量更新了，对应的公式为:
$$\{\begin{array}{l}{K}_t=\frac{P_{t|t-1}\ast H_t^T}{H_t{P}_{t|t-1}{H}^T+R_t}\\ P_t=P_{t|t-1}-K_t{H}_t{P}_{t|t-1}\end{array}$$
可得：
$${\hat{X}}_t={\hat{X}}_{t|t-1}+K_t(Z_t-H_t{\hat{X}}_{t|t-1})$$
这一串的公式推导真的很麻烦，具体可以看知乎大佬的『卡尔曼滤波：从入门到精通』

二、Python复现

我这里是用的Github上的一个开源项目，具体的应用和实例文本Notebook链接：Asll’s Heywhale

三、参考文章

[1] 知乎：卡尔曼滤波：从入门到精通
[2] Github：pykalman
[3] CSDN：关于卡尔曼滤波和卡尔曼平滑关系的理解
[4] CSDN：一文图解卡尔曼滤波(Kalman Filter)