作者 | Mockingjay 编辑 | 汽车人
原文链接:https://zhuanlan.zhihu.com/p/134595781
点击下方卡片,关注“自动驾驶之心”公众号
ADAS巨卷干货,即可获取
点击进入→自动驾驶之心【目标跟踪】技术交流群
本文只做学术分享,如有侵权,联系删文
概述
总的来说,卡尔曼滤波器是一个状态估计器,它利用传感器融合、信息融合来提高系统的精度。通常,我们要观测一个系统的状态,有两种手段。一种是通过系统的状态转移方程,并结合上一时刻的状态推得下一时刻的状态。一种是借助辅助系统(量测系统)的测量得到系统状态。这两种方式都有各自的不确定性,卡尔曼滤波可以将这两者做到最优结合(加权平均),使得我们估计的状态的不确定性小于其中任何一种。所以权重的选择至关重要,它意味着我们更信任哪一种方式得出的状态(当然是更加信任不确定性较小的状态)。
建模
比如,我们观测一辆小车的速度和位置(所以我们要观察的状态就是速度 v 和位置 p ,即状态 x=(v,p) ,这里的 p,v 都是向量),当我们有了 k-1 时刻的状态 =(,) 后,那么如果速度不变,我们就可以得到下一时刻的状态 =(,) ,其中
当然,我们也可以对小车进行控制,比如让小车加速、拐弯等等,这些都可以用一个加速度来表示: ,这样,下一时刻状态 =(,) 的表示变为:
其中:
但是,现实的情况并没有那么理想,小车可能会受到外界的各种扰动,比如,轮胎打滑,地面崎岖等等,都会使得状态转移的过程中混入噪音干扰(过程噪声),而且这个噪音往往是不可被测量的,也就是说,没办法通过建模消除噪音项。不过,往往我们可以认为这个噪音是服从零均值的高斯分布的。那么(1)式重写为
上面的状态转移方程为我们提供了观察系统状态的一种方式,同时,我们可能有一些辅助系统,比如对于这个小车,我们可以用GPS作为得到其状态的另一种方式。
那样的话,GPS每一时刻都可以提供一次当前状态的观测值 ,它与真实状态 的关系为,=+ (3)
其中的 是观测模型,它把系统真实状态空间映射成观测空间, 是噪声项,称为观测噪声——因为任何的测量系统都是有误差的,所以观测值实际上是真实值与噪声的叠加。我们同样可以认为此噪声是服从0均值的高斯分布的。即
现在我们比较这两种方式得到的系统状态,容易想到,状态转移方程得到的系统状态在演变时会非常平滑,而它的不确定度会随着迭代的进行而逐渐增大,因为误差会在一次次迭代的过程中不断累积(具体反映为估计状态的方差越来越大)。相反,由量测系统得到的状态不存在累积误差,但演变时也会很不平滑。这时我们就需要将两者得到的状态有效结合起来。这就是卡尔曼滤波做的事情了。
卡尔曼滤波
然后我们要算出测量残差:
第一项为0,因为 代表k时刻系统状态的真实值,是一个固定值,没有协方差。所以简化为:
补充内容:
另外,直观上理解:协方差矩阵代表的是数据分布的一种不确定度,这种不确定度本身不具有所谓的正负号,不可能因为加减而有所抵消。
① 全网独家视频课程
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求职交流等方向。扫码添加汽车人助理微信邀请入群,备注:学校/公司+方向+昵称(快速入群方式)
④【自动驾驶之心】平台矩阵,欢迎联系我们!