ADAS/AD开发10 - 卡尔曼滤波器与智能驾驶
前言
本来打算把基于CAN通信的五部曲（CAN通信、UDS诊断、Bootloader、Alignment、PCAN）先更完再讲其他Building blocks，但是一连几天的CAN相关内容写起来确实让人烦躁，今天又恰巧碰到一篇非常棒的卡尔曼滤波文章，临时改了注意，打算先留着Bootloader的坑，明天有时间再填。要分享的这篇文章来源于BZARG大神的文章《How a Kalman filter works， in pictures》，由微信公众号汽车ECU设计翻译整理，本文权当做自习笔记，同大家分享。无论如何，手打一遍总比读一遍印象深刻~
PS：再次重申，版权归汽车ECU设计和BZARG所有。侵权立删。
一、什么是卡尔曼滤波？
- 利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。
- 由于观测数据中包含噪声和干扰，所以最优估计也可以看做是滤波过程。
- 数据滤波是去除噪声、还原真实数据的一种数据处理技术。Kalman滤波在测量方差已知的情况下，能够从一系列处在测量噪声的数据中，估计动态系统的状态。
- 它具有占用内存空间小（除了前一个状态量外，无需保留其他历史数据）的特点，并且速度很快，适合应用于实时问题和嵌入式系统。
二、卡尔曼滤波器的应用范围
卡尔曼滤波器是一种从ADAS到AD领域都被滥用的基础滤波技术。不论是视觉上的车道线跟踪、目标跟踪，还是毫米波雷达或激光雷达的目标跟踪，概括的讲，只要用“跟踪”这个词来形容的算法，即tracking、Tracker等这种词描述的算法，基本都要用kalman滤波处理下数据。
三、卡尔曼滤波的原理
假设一个问题模型
一个在树林中到处跑的小机器人，这个小机器人有GPS定位系统，但是准确度不够（精度10米），仅依赖GPS会导致小机器人撞到树上或者掉到沟壑中。
小机器人知道它给执行器发送的控制量，可以大致估计自己应该走的轨迹，但是悲催的是，它不知道它自己的执行器的执行效果到底如何。比如，它知道它当前的环境，计算出了需要以5kph的速度、转向6°的角度来绕过一个障碍物，但是执行器执行时可能因为路面颠簸不平，导致车速快了一点、轮子因风吹影响角度偏了一点（但是小机器人自己不知道这些情况），导致发生事故。
四、状态表征描述
假设小机器人状态表征只有两个参数，位置和速度。
我们并不知道实际的位置和速度，他们之间有很多种可能正确的组合，但其中一些组合的可能性要大于其他组合。如下图（图1）.
图1 白色模糊区域表征所有可能的状态空间坐标位置
卡尔曼滤波器假设两个变量（位置、速度）都是随机的，并且服从高斯分布，即，每个变量都有一个均值μ，表征随机分布的中心（代表最最可能的状态），方差σ^2表征圆的半径，表示不确定性。假如有多个参数，那么中心只有一个，即状态均值μ，但是方差则对应不同参数，有多个方差存在，那么这个分布也不再是圆形，而是按照坐标系不同的纬度有不同的方差（不确定度）。如图2所示。
图2 状态的高斯分布
上文有一个默认的假设，就是位置和速度这两个参数是相互毫无关系的物理量。但实际上，这两个物理量是有关系的，因为下一状态的位置一定与当前的速度相关，如图3。我们可以基于旧位置来估计新位置。入股速度过高，我们可能已经移动很远；如果移动缓慢，则距离不会很远。
图3 位置与速度相关后的状态高斯分布
以上篇幅描述的参数之间的相关性，可以用协方差矩阵来表示，即矩阵中的每个元素∑ij表示第i个和第j个状态变量之间的相关度（你可能已经猜到协方差矩阵是一个对称矩阵，者意味着可以任意交换i和j）。如图4。
图4 距离和速度组成的协方差矩阵在P-V坐标系中的示意
五、使用矩阵来描述问题
基于高斯分布来建立状态变量，所以在时刻k需要两个信息：最佳估计Xk（类似于上文的均值μ），以及协方差矩阵Pk（参数不止一个，需要用矩阵形式来封装不同的参数）。
利用上式，根据当前状态（k-1时刻）来预测下一状态（k时刻）。记住，我们并不知道对下一状态的所有预测中哪个是“真实”（Ground Truth）的，但是我们的预测函数并不在乎。它总是乐此不疲地对所有的可能性进行预测，并给出新的高斯分布。预测的是包含了各个参数协方差的新高斯分布。如图5所示。
图5 从蓝色高斯分布预测到新的粉红色高斯分布
我们可以用矩阵Fk来表示这个预测过程，如图6：
图6 矩阵Fk表征预测过程
它将我们原始估计中的每个点都移动到了一个新的预测位置，如果原始估计是正确的话，这个新的预测位置就是系统下一步会移动到的位置。那我们又如何用矩阵来预测下一个时刻的位置和速度呢？下面用一个基本运动学公式来表示：
经过以上推导，我们获得了一个完整的Fk矩阵，但是Fk也需要根据每次更新的状态进行实时更新，你不能拿着过去的Fk预测矩阵去更新当前的Xk状态吧？！如果我们将分布中的每个点都乘以矩阵A，那么它的协方差矩阵Σ会怎么变化呢？
结合式（3）和式（4），我们获得到：
六、外部控制量
除了车辆自身在外部环境（路面、风吹等）的变化，一些精心设计的外部控制量也会影响小机器人的状态，例如高精地图的轨迹规划领航信息、加速踏板、减速踏板、方向盘转角等。我们可以用一个向量Uk来表示，每个元素都是一个控制量。将Uk向量加到预测方程中，做修正。
例如，我们可以假设ACC功能对前车的计算，我们获得了一个期望加速度a，根据基本的运动学方程可以得到：
Bk称为控制矩阵、Uk称为控制向量。xk的状态预测方程得到了完善。
如果我们的预测并不是100%准确，我们该怎么办？
七、外部干扰
如果系统除了有自身系统约束和已知控制量的影响，还有一些外部干扰存在，要如何建立数学模型来完善呢？
实际上，我们可以在每次预测之后，添加一些新的不确定性来建立这种与“外界”之间的不确定性模型，如图7：
图7 添加一些不确定性来完善xk的状态预测
原始预测中的每一个状态变量更新到新的状态后，仍然服从高斯分布。我们可以说Xk-1的每个状态变量移动到了一个新的服从高斯分布的区域，协方差为Qk。换句话说就是，我们将这些没有被跟踪的干扰当做协方差为Qk的噪声来处理，如图8。
图8 滤波掉不符合高斯分布的干扰
这就产生了具有不同协方差、但具有相同均值的心高斯分布。如图9所示。
图9 同一均值（同一个中心点），但是不同协方差（两种四边形）的高斯分布
我们通过简单添加Qk得到扩展的协方差，下面给出卡尔曼滤波器预测步骤的完整表达式：
总结下上文的内容，我们根据式7，得到了新的最优估计是根据上一最优估计预测得到的，并加上已知外部控制量的修正；而新的不确定性由上一不确定性预测得到，并加上外部环境的干扰。
现在，还差最后一步了，就是结合传感器测量的当前值（k_sensor状态），对预测步骤（k-1状态预测的k状态）进行比较修正，就完成了卡尔曼滤波器的设计。
八、用测量值来修正估计值
我们可能会由多个传感器来测量系统当前状态，且测量的参数也不一定都一样，有些可能位置和速度都测量，有些可能只测量速度，有些可能只测量距离。Anyway，我们要的是这些传感器测量的一些状态信息，如图10。
图10 传感器测量的状态信息与kalman预测的高斯分布的比较
我们用Hk表示传感器读取的当前状态数据，如图11：
图11 用Hk表征传感器测量所得的数据
我们可以计算出传感器读取的分布，用之前的表示方法如下式所示：
Kalman Filter的一大优点就是能处理传感器噪声，因为传感器得到的数据或多或少有点不靠谱，并且原始估计中的每个状态可以和一定范围内的传感器读取对应起来，如图12。
图12 用卡尔曼滤波器的预测值来剔除传感器的一些不靠谱数据
从测量到的传感器数据中，我们大致能猜到系统当前处于什么状态。但是由于存在不确定性，某些状态可能比我们得到的读数更接近真实状态，如图13所示。
图13 传感器的高斯分布
我们将传感器噪声用协方差Rk表示，该分布的均值就是我们读取到的传感器数据，称之为Zk。现在我们有了两个高斯分布，一个是在预测值附近，一个是在传感器读数附近，如图14所示。
图14 融合预测高斯分布（粉色部分）和传感器高斯分布（绿色部分）
我们必须在预测值（粉色）与传感器测量值（绿色）之间找到最优解。那么，我们有可能的状态是什么？对于任何可能的读数（z1，z2），有两种情况：1 传感器的测量值；2 由前一状态得到的预测值。如果我们想知道这两种情况都可能发生的概率，将这两个高斯分布向乘就可以了，如图15。剩下的就是剩余部分了，如图16所示。这个重叠部分的均值就是两个估计最可能的值，也就是给定的所有信息中的最优估计。值得一提的是，这个重叠区域看起来就像是另一个高斯分布呢。
图15 预测高斯分布与测量高斯分布的相乘
图16 剩余部分的均值就是两个估计最可能的值
如你所见，把两个具有不同均值和协方差的高斯分布相乘，就得到了一个新的具有独立均值和协方差的高斯分布！即融合高斯分布。
九、融合高斯分布
先以意味高斯分布来分析比较简单，具有方差和均值的高斯曲线用下式表示：
如果把两个服从高斯分布的函数相乘会得到什么呢？
图17 两个高斯函数相乘，得到蓝色叠加部分
将式9代入到式10中，注意重新归一化，使总概率保持为1，可以得到：
再将式11中的两个式子相同的部分用k表示：
下面进一步将式12和式13写成矩阵的形式，如果∑表示高斯分布的协方差，μ表示每个维度的均值，即：
矩阵K就是kalman滤波器的增益。
我们将预测部分和测量部分的高斯分布进行整合，放到式15中算出它们之间的重叠部分：
由此获得卡尔曼滤波的增益为：
最后，将式16和式17的两边同时乘矩阵的逆，得到最终的卡尔曼滤波器的数学模型：
其中，Xk’就是新的最优估计。我们可以将它和Pk’放到下一个预测和更新方程中不断迭代。
总结
上述公式中，你在软件中只需要用到式7、式18和式19。
按照Dataflow，捋一下卡尔曼滤波器的工作流程如下：
（完）