卡尔曼滤波原理及代码实战

简介

我们可以将卡尔曼滤波看做一种运动模型，它可以在任何含有不确定信息的动态系统中，对系统的下一步走向做出一定的预测，无论是在单目标还是多目标领域都是很常用的一种算法。它最大的优点就是采用递归的方式来解决线性滤波的问题，只需要当前的观测值和前一个周期的预测值就能够进行状态估计。

1.原理介绍

场景假设

假设有一个小车在路上行驶，小车的初始位置和速度用一个状态向量表示：
$$\begin{gathered} {\hat{x}}_t=[position,velocity] \\ 简写为 \\ {\hat{x}}_t=[p_t,v_t] \end{gathered}$$
因为用两个变量表示了状态向量，所以系统中这两个变量的不确定性和相关性可以用一个协方差矩阵来表示：
$$P_t=\left[\begin{array}{c}{\sum }_{pp}{\sum }_{pv}\\ {\sum }_{vp}{\sum }_{vv}\end{array}\right]$$
比如前一时刻的速度$V_{t-1}$和下一时刻的速度$V_t$存在着某种关联，不可能从0突变成光速；

比如前一时刻的速度$P_{t-1}$和下一时刻的速度$P_t$也存在着某种关联，不可能从我国某地突然变成火星某地。

(1).下一时刻的状态

当小车在做匀速运动时，小车下一时刻的位置和速度如下：
$$\{\begin{array}{l}{p}_t=p_{t-1}+v_{t-1}\Delta t\\ v_t=v_{t-1}\end{array}$$

矩阵形式为：
$$\left[\begin{array}{c}{p}_t\\ v_t\end{array}\right]=\left[\begin{array}{c}1\Delta t\\ 01\end{array}\right]\left[\begin{array}{c}{p}_{t-1}\\ v_{t-1}\end{array}\right]=F_t\left[\begin{array}{c}{p}_{t-1}\\ v_{t-1}\end{array}\right]$$

即：$${\hat{x}}_t=\left[\begin{array}{c}1\Delta t\\ 01\end{array}\right]{\hat{x}}_{t-1}=F_t{\hat{x}}_{t-1}$$
记状态转移矩阵为，表示小车状态随时间的变化:
$$F_t=\left[\begin{array}{c}1\Delta t\\ 01\end{array}\right]$$
系统的不确定性和相关性用$P_t$表示，而下一时刻的协方差矩阵可表示为（T表示矩阵的转置）：
$$P_t=F_t{P}_{t-1}{F}_t^T$$

(2).增加系统的内部控制

如果对小车进行控制，比如加速和减速，假设它的加速度为$a_t$，则小车下一时刻的位置和速度是：

$$\{\begin{array}{l}{p}_t=p_{t-1}+v_{t-1}\Delta t+\frac{1}{2}{a}_t\Delta t^2\\ v_t=v_{t-1}+a_t\Delta t\end{array}$$
矩阵形式为：
$$\left[\begin{array}{c}{p}_t\\ v_t\end{array}\right]=\left[\begin{array}{c}1\Delta t\\ 01\end{array}\right]\left[\begin{array}{c}{p}_{t-1}\\ v_{t-1}\end{array}\right]+\left[\begin{array}{c}\frac{\Delta t^2}{2}\\ \Delta t\end{array}\right]a_t=F_t\left[\begin{array}{c}{p}_{t-1}\\ v_{t-1}\end{array}\right]+B_t{u}_t$$
记状态控制矩阵为$B_t=\left[\begin{array}{c}\frac{\Delta t^2}{2}\\ \Delta t\end{array}\right]$，表明加速度如何改变小车的状态；

记状态控制向量为 $u_t$，表明控制的力度大小和方向。

(3).考虑运动系统外部的影响

小车运动时有很多因素可以对它的位置速度造成影响，比如风俗，路况等，假设外部对小车造成的系统状态误差为$w_t$，并且它服从高斯分布$w_{t}N(0,Q_t)$，代入之前的小车运行控制方程，可以得到最终完整的状态预测方程：
$$\begin{gathered} {\hat{x}}_t=F_t{\hat{x}}_{t-1}+B_t{u}_t+w_t \\ {P}_t=F_t{P}_{t-1}{F}_t^T+Q_t \end{gathered}$$
除了小车运动的场景，其他物体的状态变化

一般情况下，假设$w_t$为0即可，将上述公式规范化得到：
$$\begin{gathered} x=Fx+Bu \\ P=FP{F}^T+Q \end{gathered}$$

(4).后验估计：预测结果与观测结果的融合

在等3步中我们预测出了小车的下一时刻的运动状态，将它的结果称为预测结果，也称为先验估计；不过实际中，汽车上会有里程表、GPS等设备也会提供小车下一时刻的运动状态，称其结果为观测结果。

在实际情况中，预测结果和观测结果都有一定的误差，它们的结果都不一定十分准确，而卡尔曼滤波最重要的作用就是将两者融合，充分利用两者的不确定性得到更加准确的结果，称之为后验估计。

卡尔曼增益K

融合阶段需要用到一个关键的系数卡尔曼增益K，其公式如下（推导过程略）：
$$K=\frac{P{H}^T}{(HP{H}^T)+R}$$
P表示变量的不确定性和相关性的协方差矩阵；

R表示传感器观测结果的噪声的协方差矩阵，因为观测值的结果也不一定完美；

H表示预测值和观测值之间的某种关系矩阵，因为它们的结果存在的一定交合；

整体来说，卡尔曼增益表示对观测结果借鉴的程度，相当于一个权重项，让算法知道怎么利用观测与预测进行估计，目的就是让最优估计值的方差更小，是卡尔曼滤波的核心。

2.卡尔曼滤波计算过程

卡尔曼滤波的计算一般分为两步：预测阶段和更新阶段。

(1).预测阶段（先验估计阶段）

$$\begin{gathered} x=Fx+Bu \\ P=FP{F}^T+Q \end{gathered}$$

(2).更新阶段（后验估计阶段）

$$\begin{gathered} K=P{H}^T((HP{H}^T)+R{)}^{-1} \\ x=x+(K(z-(Hx))) \\ P=(I-(KH))P \end{gathered}$$

x表示系统状态；

z表示传感器的观测结果；

F表示状态转移矩阵；

B表示状态控制矩阵；

u表示状态控制向量；

H表示预测和观测的关系矩阵;

P表示变量的不确定性和相关性的协方差矩阵；

R表示传感器观测结果的噪声的协方差矩阵；

Q表示不确定因素对系统预测结果造成误差的协方差矩阵。

Q和R根据实际情况来定，不断调优根据效果选择一个最优解；

在实际计算中，我们需要关注的是系统的状态x，和它的误差协方差矩阵P，卡尔曼增益K。

3.代码举例