卡尔曼滤波与目标追踪

卡尔曼滤波部分我打算分三节（三次博客的内容）：

• 卡尔曼滤波与行人状态估计
• 扩展卡尔曼滤波（EKF）与传感器融合
• 过程模型，无损卡尔曼滤波（UKF）与车辆状态轨迹

为什么要学卡尔曼滤波？

卡尔曼滤波以及其扩展算法能够应用于目标状态估计，如果这个目标是行人，那么就是行人状态估计（或者说行人追踪），如果这个目标是自身，那么就是车辆自身的追踪（结合一些地图的先验，GPS等数据的话就是自身的定位）。在很多的无人驾驶汽车项目中，都能找到卡尔曼滤波的扩展算法的身影（比如说EKF，UKF等等）。本节我们从最简单的卡尔曼滤波出发，完整的理解一遍卡尔曼滤波的推导过程，并实现一个简单的状态估计Python程序。

卡尔曼滤波是什么？

我们通常要对一些事物的状态去做估计，为什么要做估计呢？因为我们通常无法精确的知道物体当前的状态。为了估计一个事物的状态，我们往往会去测量它，但是我们不能完全相信我们的测量，因为我们的测量是不精准的，它往往会存在一定的噪声，这个时候我们就要去估计我们的状态。卡尔曼滤波就是一种结合预测（先验分布）和测量更新（似然）的状态估计算法。

一些概率论的知识基础

先验概率 P ( X ) ：仅仅依赖主观上的经验，事先根据已有的只是的推断
后验概率 P ( X ∣ Z )：是在相关证据或者背景给定并纳入考虑以后的条件概率
似然 P ( Z ∣ X ) ：已知结果区推测固有性质的可能性

贝叶斯公式：

P(A∣B)= P(A)×P(B∣A) / P(B)
后验分布正比于先验分布乘以似然。
​

卡尔曼滤波完整推导

一个简单的例子

若干年后，我们的可回收火箭要降落到地球，我们比较关心的状态就是我们的飞行器的高度了，飞行器的高度就是我们想要估计的状态，我们会通过一些传感器去测量我们当前的高度信息，比如说使用气压计。假如我们每一次测量，当前高度都变成上一次测量的 95 %，那么我们就可以得到如下关系：

Height^(t)=0.95×Height ^(t−1)

我们可以使用递归来表示这样的公式——为了计算我们当前的高度，我们必须知道我们上一个测量的高度，以此递推，我们就会推到我们的飞行器的初始高度。

但是，由于我们的测量往往来自于一些传感器（比如说GPS，气压计），所以测量的结果总是带有噪声的，这个噪声是有传感器本身引起的，那么我们的表达式就变成了：

Measurement ^(t) =Height^(t) + Noise ^(t)
这个噪声我们称之为测量噪声 (Measuremen Noise)。通常来说，这种噪声都满足高斯分布。我们用符号描述以上两部分：

第一个式子是我们的 过程模型，是我们的经验（比如说一些运动模型，牛顿力学等等），我们用这种过程模型去 预测 我们考察的事物的状态（在没有任何信息的情况下，或者说在没有任何测量数据的情况下）；第二个式子是测量的表达式，它大致描述了我们测量的组成成分，我们用这个测量去 更新 我们对状态的估计。 其中 x_k是我们的飞行器当前的状态， x_k-1 是我们上一个状态（注意状态和测量的区别），z_k是我们当前对飞行器的测量，v_k使我们当前的测量噪声， a 是一个常数，在我们这个例子里面就是 0.95。

很显然，现实中的运动不会像我们这个简单的过程模型一样高度按比例缩小。在这里为了简化，我们先假设我们的模型挺好的，能够大致描述出这颗“神奇火箭”的运动规律，只是偶尔会存在一定的偏差（比如所空气湍流的影响），那么我们在计 x_k时在加一个噪声来描述我们的过程模型与实际运动的差异，这个噪声我们称之为过程噪声 (Process Noise)，我们用 w_k 来表示这种噪声，那么x_k 的计算公式就变成：

x_k​ =ax_k−1+w_k

为了简化，后面的分析我们会先忽略过程噪声，但是我们在传感器融合的部分会将过程噪声重新考虑进来。

状态估计

因为我们是要估计飞行器的状态（高度），所以我们把测量的公式变换成：

x_k​ =z_k-v_k

显然，在这里 v_k 是没办法知道的，卡尔曼滤波通过同时考虑上一状态值和当前的测量值来获得对当前状态值的估计。一般我们用 $\hat{x}$来表示对状态 x 的估计。那么$\hat{x}$_k就表示当前状态的估计。下面我们可以用如下公式来描述卡尔曼滤波如何结合上一个估计和和现在的测量来产生对当前的估计：

这里的 g_k 叫做 卡尔曼增益（Kalman Gain），它描述的是之前的估计和当前的测量对当前的估计的影响的分配权重。为了理解，我们考虑极端的例子，如果 g_k = 0 ,也就是说增益为 0，那么：

$\hat{x}$_k = z_k

即我认为当前的测量非常可信，我彻底接受它作为我当前状态的估计。那么当 g_k 介于 ( 0 , 1 )，则表示对两者的权重分配。

计算卡尔曼增益

那么如何计算卡尔曼增益呢？我们使用一种间接的方法，我们虽然不知道测量噪声 v_k 的值，但是我们知道它的均值，前面我们提到，测量噪声来自传感器本身，并且符合高斯分布，所以我们能够从传感器厂商那里获得测量噪声的均值 r ，那么 g_k可以表示为：

g_k=p_k-1 /(p_k-1 + r)

其中 p_k 叫预测误差，表达式为：

p_k = (1 - g_k ) p_k-1

那么怎么理解 p_k和 g_k呢？

那么假设前一次的预测误差 p_k-1=0， 那么根据公式，当前的增益g_k=0，一维着舍弃掉当前的测量而完全采用上一个时刻的估计，如果p_k-1=1, 那么增益变成 1 / (1 + r)通常 r 是个很小的数值，所以增益为1，所以完全接受这一次的测量作为我们的估计（因为上一次的的预测误差太大了，为1，所以一旦拿到了新的测量，如获至宝，就干脆把不准确的上次的估计舍弃掉了）.

对于下面的公式的分析是一样的，我们考虑极端的例子，当增益为 0 ，p_k = p_k-1，因为我们彻底舍弃掉了本次的测量，所以本次的预测误差只能接受上一次的。当增益为 1 ，p_k= 0 。

预测和更新

那么现在我们有关于当前的火箭高度状态的两个公式了，它们分别是：
x_k = a x_k-1
和

那么我们到底要用哪一个呢？答案是我们都用，第一个公式我们称之为预测，是基于一些先验的知识（比如说运动模型，牛顿力学等等）觉得我们的状态应该是这样的，而第二个公式呢，就是我们基于我们“不完美的”的传感器的测量数据来更新我们对状态的估计。另为，预测，理论上只考虑了一个固定的过程模型和过程噪声，但是由于我们现在是对机械的状态进行估计，在预测过程中需要对机械本身的控制建模， 我们在预测部分再新增一个控制信号，我们用 b u_k 表示。实际的传感器测量除了会有测量噪声v_k 以外，还会存在一定的关于真实状态的缩放，因此我们使用 x_k 表示测量时通常还会在其前面加一个缩放系数 c。 结合这写我们就可以得到卡尔曼滤波预测和更新过程了：

预测

卡尔曼滤波更新的过程为：

使用线性代数的方法来表示预测和更新
预测

卡尔曼滤波更新的过程为：

至此，卡尔曼滤波的完整推导就结束了。下面，我们来看看卡尔曼滤波在无人汽车的感知模块的应用。

卡尔曼滤波算法为什么会叫滤波算法？

以一维卡尔曼滤波为例，如果我们单纯的相信测量的信号，那么这个信号是包含噪声的，是很毛糙的，但是当我们运行卡尔曼滤波算法去做估计，我们估计的信号会很光滑，看起来似乎滤掉了噪声的影响，所以称之为滤波算法。实际上，卡尔曼滤波不仅仅过滤掉了测量信号的噪声，它同时也结合了以往的估计，卡尔曼滤波在线性问题中被证明是最优估计。

参考：
https://blog.csdn.net/AdamShan/article/details/78248421