作者 | ZZZzz 编辑 | 汽车人
原文链接：https://zhuanlan.zhihu.com/p/642457881
点击下方卡片，关注“自动驾驶之心”公众号
ADAS巨卷干货，即可获取
点击进入→自动驾驶之心【多传感器融合】技术交流群
本文只做学术分享，如有侵权，联系删文
前言
自动驾驶场景中需要持续观测、预测目标的运动和发展情况，以对当前状态采取更适合的决策。为了估计待测目标的状态，如距离和速度，最直接的方式是用传感器测量，如相机和毫米波雷达。但由于测量的误差和噪声的存在，又不能完全相信测量值。此时可以采用概率学和统计学的方法来分析统计和估计状态量。卡尔曼滤波就是这样一种结合预测（先验分布）和测量更新（似然估计）的状态估计算法。
卡尔曼滤波是一种递推算法，每次递推主要包括预测和修正两个步骤。本文说明的是LKF（Linear Kalman Filter），即线性卡尔曼滤波，下文中以“LKF”替代。
CA模型的卡尔曼滤波
状态转移方程
在真实道路场景中，通常假设待测目标满足匀加速运动（CA模型）或匀速旋转运动（CT模型）。若为匀加速运动（CA模型），待测目标的状态可以用如下数学方程表示 ：
状态转移方程（运动方程） 如下：
其中:
(1)为状态转移矩阵，为目标的运动建模，如匀速、匀加速、转弯运动等；
(2)为k时刻系统的输入量，运动模型里一般不考虑此项，化简为0；
(3)为过程噪声，是对模型建立过程中不确定性与随机扰动的一个综合的抽象描述且服从正态分布w ~ N(0,Q)。
(4) Q为过程噪声的协方差矩阵，由不确定的噪声引起。Q值越小系统越容易收敛，对模型预测的值信任度越高，但是太小则容易发散。
状态转移方程可简化为：
扩展为：
将上式展开可以得到：
原来的模型表达式就变成了：
由上式可知，其相应的过程噪声为：
由上文知，过程噪声 w N(0,Q) 关于Q的公式推导有以下两种选择：
此外，还有一种计算方式，即直接对离散化的CA模型进行积分得到：
第一种计算方法会导致协方差奇异，但具体影响不大。此处选择了第二种计算方法。
测量方程
目标运动的测量方程如下：
其中H为测量矩阵，vk为测量噪声，和状态方程中的过程噪声wk类似，也服从如下正态分布： N(0,R)，R为测量噪声的协方差矩阵。
使用传感器可以直接测量得到目标位置、速度及加速度，根据状态表达式，测量矩阵H可以表示为：
测量噪声的协方差矩阵R为：
定义测量值与估计值之间的估计误差为：
不考虑噪声时，状态空间方程为：
定义卡尔曼增益 为：
综上所述，LKF共分为下述五个步骤：
计算先验估计
计算先验误差协方差矩阵
计算卡尔曼增益
计算后验估计
更新误差协方差矩阵
CT模型的卡尔曼滤波
状态转移方程
对于匀速旋转运动（CT模型），在上述基础上，还需增加偏航角和偏航角速度两个状态变量。
将上式展开可以得到:
CT模型的状态估计中，过程噪声是角速度的变化率，由分析不难得知，对于转弯运动：
则原来的模型表达式就变成了：
由上式可知，其响应的过程噪声为：
此外，还有一种计算方式，即直接对离散化的CT模型进行积分得到：
Q矩阵计算推导如下：(不重要，可忽略不看)
测量方程
目标运动的测量方程如下：
其中H为测量矩阵，vk为测量噪声，和状态方程中的过程噪声wk类似，也服从如下正态分布：vk ~ N(0,R)，R为测量噪声的协方差矩阵。
使用传感器可以直接测量得到航向角θ，根据状态表达式，测量矩阵H可以表示为：
测量噪声的协方差矩阵R为：
① 全网独家视频课程
BEV感知、毫米波雷达视觉融合、多传感器标定、多传感器融合、多模态3D目标检测、点云3D目标检测、目标跟踪、Occupancy、cuda与TensorRT模型部署、协同感知、语义分割、自动驾驶仿真、传感器部署、决策规划、轨迹预测等多个方向学习视频（扫码学习）
视频官网：www.zdjszx.com
② 国内首个自动驾驶学习社区
近2000人的交流社区，涉及30+自动驾驶技术栈学习路线，想要了解更多自动驾驶感知（2D检测、分割、2D/3D车道线、BEV感知、3D目标检测、Occupancy、多传感器融合、多传感器标定、目标跟踪、光流估计）、自动驾驶定位建图（SLAM、高精地图、局部在线地图）、自动驾驶规划控制/轨迹预测等领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频，期待交流！
③【自动驾驶之心】技术交流群
自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多模态感知、Occupancy、多传感器融合、transformer、大模型、点云处理、端到端自动驾驶、SLAM、光流估计、深度估计、轨迹预测、高精地图、NeRF、规划控制、模型部署落地、自动驾驶仿真测试、产品经理、硬件配置、AI求职交流等方向。扫码添加汽车人助理微信邀请入群，备注：学校/公司+方向+昵称（快速入群方式）
④【自动驾驶之心】平台矩阵，欢迎联系我们！
