卡尔曼滤波器介绍

最经典的跟踪算法莫过于卡尔曼老爷子在1960年提出的卡尔曼滤波器。在无人车领域，卡尔曼滤波器除了应用于障碍物跟踪外，也在车道线跟踪、障碍物预测以及定位等领域大展身手。

工作原理

简单来讲，卡尔曼滤波器就是根据上一时刻的状态，预测当前时刻的状态，将预测的状态与当前时刻的测量值进行加权，加权后的结果才认为是当前的实际状态，而不是仅仅听信当前的测量值。

前提假设

 卡尔曼滤波器是基于在时域中离散的线性动力系统(在此我们考虑离散 的系统)。卡尔曼滤波器是基于马尔科夫链的模型,其建立在线性运算符的 基础上,过程噪声和观测噪声符合高斯分布,且过程噪声和观测噪声不相关。

首先滤波分为线性和非线性两种，Kalman filter 是在满足五个假设条件（推荐 estimation with application to tracking and navigation）下才成立的，且动态方程和量测方程都是线性的。KF在MMSE条件下是最优的，由于它是线性的，所以也是LMMSE最优。五个公式主要分为两部分：预测和更新。抓住这两个公式即可。 KF 假设状态服从高斯分布，状态方程（动态和量测）为线性，过程噪声和量测噪声为高斯白噪声；得到在MMSE（最小化均方误差）下的最优解；

初始化（Initialization）

假设有个小车在道路上向右侧匀速运动，我们在左侧安装了一个测量小车距离和速度传感器，传感器每1秒测一次小车的位置s和速度v，如下图所示。

我们用向量xt来表示当前小车的状态，该向量也是最终的输出结果，被称作状态向量（state vector）：

既然我们是在对真实值进行估计，那么就理应考虑到噪声的影响。实践中，我们通常都是假设噪声服从一个0均值的高斯分布，即Noise~Guassian(0,σ)。例如对于一个一维的数据进行估计时，若要引入噪声的影响，其实只要考虑其中的方差σ即可。当我们将维度提高之后，为了综合考虑各个维度偏离其均值的程度，就需要引入协方差矩阵。

由于测量误差的存在，传感器无法直接获取小车位置的真值，只能获取在真值附近的一个近似值，可以假设测量值在真值附近服从高斯分布。如下图所示，测量值分布在红色区域的左侧或右侧，真值则在红色区域的波峰处。

由于是第一次测量，没有小车的历史信息，我们认为小车在1秒时的状态x与测量值z相等，表示如下：

公式中的1表示第1秒。

 

预测（Prediction）

预测是卡尔曼滤波器中很重要的一步，这一步相当于使用历史信息对未来的位置进行推测。根据第1秒小车的位置和速度，我们可以推测第2秒时，小车所在的位置应该如下图所示。会发现，图中红色区域的范围变大了，这是因为预测时加入了速度估计的噪声，是一个放大不确定性的过程。

根据小车第一秒的状态进行预测，得到预测的状态xpre：

其中，pre是prediction的简称；时间间隔为1秒，所以预测位置为距离+速度*1；由于小车做的是匀速运动，因此速度保持不变。

观测（Measurement）

 

在第2秒时，传感器对小车的位置做了一次观测，我们认为小车在第2秒时观测值为z2，用向量表示第2秒时的观测结果为：

很显然，第二次观测的结果也是存在误差的，我们将预测的小车位置与实际观测到的小车位置放到一个图上，即可看到：

图中红色区域为预测的小车位置，蓝色区域为第2秒的观测结果。

 

很显然，这两个结果都在真值附近。为了得到尽可能接近真值的结果，我们将这两个区域的结果进行加权，取加权后的值作为第二秒的状态向量。

 

为了方便理解，可以将第2秒的状态向量写成：

其中，w1为预测结果的权值，w2为观测结果的权值。两个权值的计算是根据预测结果和观测结果的不确定性来的，这个不确定性就是高斯分布中的方差的大小，方差越大，波形分布越广，不确定性越高，这样一来给的权值就会越低。

加权后的状态向量的分布，可以用下图中绿色区域表示：

你会发现绿色区域的方差比红色区域和蓝色区域的小。这是因为进行加权运算时，需要将两个高斯分布进行乘法运算，得到的新的高斯分布的方差比两个做乘法的高斯分布都小。

两个不那么确定的分布，最终得到了一个相对确定的分布，这是卡尔曼滤波的一直被推崇的原因。

再预测，再观测（Prediction & Measurement）

第1秒的初始化以及第2秒的预测、观测，实现卡尔曼滤波的一个周期。

同样的，我们根据第2秒的状态向量做第3秒的预测，再与第3秒的观测结果进行加权，就得到了第3秒的状态向量；再根据第3秒的状态向量做第4秒的预测，再与第4秒的观测结果进行加权，就得到了第4秒的状态向量。以此往复，就实现了一个真正意义上的卡尔曼滤波器。

以上就是卡尔曼滤波器的感性分析过程，下面我们回归理性，谈谈如何将以上过程写成代码。

公式

下面7个公式就是卡尔曼滤波器的理性描述，使用下面7个公式，就能够实现一个完整的卡尔曼滤波器。

 

预测（Prediction）公式

首先是公式

这里的x为状态向量，通过左乘一个矩阵F，再加上外部的影响u，得到预测的状态向量x'。这里的F成为状态转移矩阵（state transistion matrix）。以2维的匀速运动为例，这里的x为

对于匀速运动模型，根据中学物理课本中的公式s1 = s0 + vt，经过时间△t后的预测状态向量应该是

由于假设当前运动为匀速运动，加速度为0，加速度不会对预测造成影响，即

如果换成加速或减速运动模型，就可以引入加速度a_x和a_y，根据s1 = s0 + vt + at^2/2这里的u会变成：

作为入门课程，这里不讨论太复杂的模型，因此公式

最终将写成

 

 

第二个公式

该公式中P表示系统的不确定程度，这个不确定程度，在卡尔曼滤波器初始化时会很大，随着越来越多的数据注入滤波器中，不确定程度会变小，P的专业术语叫状态协方差矩阵（state covariance matrix）；这里的Q表示过程噪声（process covariance matrix），即无法用x'=Fx+u表示的噪声，比如车辆运动时突然到了上坡，这个影响是无法用之前的状态转移估计的。

 

以激光雷达为例。激光雷达只能测量点的位置，无法测量点的速度，因此对于激光雷达的协方差矩阵来说，对于位置信息，其测量位置较准，不确定度较低；对于速度信息，不确定度较高。因此可以认为这里的P为：

由于Q对整个系统存在影响，但又不能太确定对系统的影响有多大。工程上，我们一般将Q设置为单位矩阵参与运算，即

根据以上内容和公式

 

观测（Measurement）

观测的第一个公式是

这个公式计算的是实际观测到的测量值z与预测值x'之间差值y。不同传感器的测量值一般不同，比如激光雷达测量的位置信号为x方向和y方向上的距离，毫米波雷达测量的是位置和角度信息。因此需要将状态向量左乘一个矩阵H，才能与测量值进行相应的运算，这个H被称为测量矩阵（Measurement Matrix）。

 

激光雷达的测量值为

其中xm和ym分别表示x方向上的测量（measurement）值。

由于x'是一个4*1的列向量，如果要与一个2*1的列向量z进行减运算，需要左乘一个2*4的矩阵才行，因此整个公式最终要写成：

即，对于激光雷达来说，这里的测量矩阵H为

求得y值后，对y值乘以一个加权量，再加到原来的预测量上去，就可以得到一个既考虑了测量值，又考虑了预测模型的位置的状态向量了。

那么y的这个权值该如何取呢？

再看接下里的两个公式

这两个公式求的是卡尔曼滤波器中一个很重要的量——卡尔曼增益K（Kalman Gain），用人话讲就是求y值的权值。第一个公式中的R是测量噪声矩阵（measurement covariance matrix），这个表示的是测量值与真值之间的差值。一般情况下，传感器的厂家会提供该值。S只是为了简化公式，写的一个临时变量，不要太在意。

看最后两个公式

这两个公式，实际上完成了卡尔曼滤波器的闭环，第一个公式是完成了当前状态向量x的更新，不仅考虑了上一时刻的预测值，也考虑了测量值，和整个系统的噪声，第二个公式根据卡尔曼增益，更新了系统的不确定度P，用于下一个周期的运算，该公式中的I为与状态向量同维度的单位矩阵。

 

在这个基础上，业内还有扩展卡尔曼滤波器和无迹卡尔曼滤波器，它们与经典卡尔曼滤波器的最大区别是状态转移矩阵F和测量矩阵H的不同，剩下的跟踪过程依然需要使用前面介绍的7个公式。

只要你能够写出某个模型的F、P、Q、H、R矩阵，任何状态跟踪的问题都将迎刃而解

 

总结：

F：状态转移矩阵，u：外部控制量(比如加速度),

P的专业术语叫状态协方差矩阵（state covariance matrix）,表示不确定程度；这里的Q表示过程噪声（process covariance matrix），这个是无法用之前的状态转移估计的；

H被称为测量矩阵（Measurement Matrix）

K ：卡尔曼增益（Kalman Gain），用人话讲就是求y值的权值(实际观测到的测量值z与预测值x'之间差值y)

R是测量噪声矩阵（measurement covariance matrix），这个表示的是测量值与真值之间的差值。一般情况下，传感器的厂家会提供该值。S只是为了简化公式，写的一个临时变量

 

 

步骤

理解下面这个五个方程的含义就可以了：

 

x^−k=Ax^k−1+Buk−1x^k−=Ax^k−1+Buk−1

P−k=APk−1AT+QPk−=APk−1AT+Q

Kk=P−kHT(HP−kHT+R)−1Kk=Pk−HT(HPk−HT+R)−1

x^k=x^−k+Kk(zk−Hx^−k)x^k=x^k−+Kk(zk−Hx^k−)

Pk=P−k−KkHP−kPk=Pk−−KkHPk−

• 公式（步骤）1：由上一时刻的状态预测当前状态，加上外界的输入。
• 公式（步骤）2：预测过程增加了新的不确定性QQ，加上之前存在的不确定性。
• 公式（步骤）3：由预测结果的不确定性P−kPk−和观测结果的不确定性RR计算卡尔曼增益（权重）。
• 公式（步骤）4：对预测结果和观测结果做加权平均，得到当前时刻的状态估计。
• 公式（步骤）5：更新PkPk，代表本次状态估计的不确定性。

需要注意的是，在定位中状态xkxk是一个向量，除了坐标外还可以包含速度，比如xk=(坐标x，坐标y，速度x，速度y)xk=(坐标x，坐标y，速度x，速度y)，状态是向量而不仅仅是一个标量，上面的几个公式中的矩阵乘法实际上是同时对多个状态进行计算，表示不确定性的方差也就成了协方差矩阵。

 

 

卡尔曼滤波五大更新方程

有时间更新和状态更新：

时间更新

状态更新

    Kalman Filter的算法中，从初始状态开始，不断计算当前状态的均值和方差来迭代，直至系统结束。

​    卡尔曼滤波有以下两种作用。第一，用作单种数据滤波，那么将数据作为测量量创数模型，卡尔曼滤波通过上一次的值估计出下一次的值，然后将此次的估计值和测量值分别取一定的权重（卡尔曼增益），从而求出这次的最优质。第二，多数据的融合，将一种数据作为测量量，另一种数据作为估计值进行融合。
 

 

   参考文献：

1.  运动目标跟踪（一）--搜索算法预测模型之KF,EKF,UKF（文献1，主要是原理的梳理，十分详细）

2. [opencv]Kalman滤波跟踪（文献2，主要是公式与opencv对应关系的讲解）

3.  学习OpenCV2——卡尔曼滤波(KalmanFilter)详解（文献3，三个例子代码）

4.https://zhuanlan.zhihu.com/p/45238681

5.https://blog.csdn.net/coming_is_winter/article/details/79048700

徐亦达博士关于卡尔曼滤波的公开课，http://v.youku.com/v_show/id_XMTM2ODU1MzMzMg.html

CSDN，再谈卡尔曼滤波–五大公式，https://me.csdn.net/woaizgw)https://blog.csdn.net/woaizgw/article/details/73648578

CSDN，卡尔曼滤波算法的深入理解，https://blog.csdn.net/L_smartworld/article/details/82145013