作者 | 荒唐病人 编辑 | 汽车人
原文链接：https://www.zhihu.com/question/47559783/answer/2980976068
点击下方卡片，关注“自动驾驶之心”公众号
ADAS巨卷干货，即可获取
点击进入→自动驾驶之心【目标跟踪】技术交流群
本文只做学术分享，如有侵权，联系删文
Kalman 系列
认识kalman是大二做车时知道这个东西可以用来做传感器滤波，后来慢慢知道KALMAN大神几乎是创立现代控制方法，而一开始的kalman滤波器是为了阿波罗计划而提出的。
相比于复杂的数学公式和艰深的理论，我更喜欢直观的理解，尤其是用图的方式。
卡尔曼滤波器提出的背景
火箭在发生过程中需要时刻预测自己的状态（位置，速度），我们可以用一些传感器获得这两个量，但是由于存在电离层和大气干扰，传感器的测量值有时误差会非常大，我们需要一种方法，预测一个新的更加准确的、且不完全依赖传感器返回值的状态信息。
事实上，如果传感器的返回值非常离谱，那我们完全有理由不相信此时的数据，因为火箭运动需要 符合某个运动方程，即使存在一些干扰，火箭的状态也应该是在某个合理的范围内的。
一个顺理成章的思路是：把传感器观测值和理论预测值做加权平均。既不完全相信传感器，也不完全相信理论模型
上式的意思就是把由前n-1个时刻得到的n时刻的理论值和n时刻的观测值进行加权平均。
接下来的问题是：如何确定 α?
——为了解决这个问题，我们不妨从最简单的情况出发，即设计一个1维kalman滤波器下手。
一维KALMAN滤波器
假设我们只需要观测一个值，即设计一个1维Kalman Filter。
由于理论模型和观测都是存在相应干扰的， 假设二值均服从正态分布（我们生活中几乎到处都是正态分布）。
红色是理论值分布，绿色是观测值分布。(这里注意，理论值也是一个概率分布，这是因为我们的理论模型肯定不能完全吻合实际情况)
新的值应该结合这两者，即像蓝色分布那样。
现在只考虑均值 μ,一个很直观的思路是：变化更平稳（不确定度更低）的值可信度应该更大，即方差越小权重应该越高。同时保证总权重为1，则有：
(注意，上式中权重的分母是为了保证总和为1，而根据方差越大权重越小的思想,,的系数分子部分正好是相互反过来的. 这里 σ表示方差，但是没有加平方，是为了简化表示，便于理解，请勿和标准差混淆。)
我们现在有了一种计算 α\alpha\alpha 的方法，但是这只是直观上的，现在尝试用一种更符合数学逻辑的思路: 希望新的值的不确定度最低，即使得新的值的方差取最小值（计算融合结果的方差表达式，并使导数为0）
ps：这里需要读者有一点点概率论基础，设x,y是两个概率分布，并且有 y=ax,那么二者的方差存在关系
可以发现这样算出来的答案和之前的想法不谋而合了！
现在我们根据,可以计算权重 了，那如何得到,?
理论模型值的概率分布 和观测值的概率分布只能通过大量数据采样得到，这要求我们在应用上述方法进行滤波前，应该通过大量观察对理论模型和观测数据的分布有一个准确的了解。实际工程我们一般假设传感器是具有某个确定分布的，这个分布可以对传感器进行一段时间的观测得到（如slam中经常会让系统静止两小时以计算传感器的分布）。而系统方差是会随着我们每一次进行kalman融合而改变的（因为状态矩阵a的影响），我们需要持续的计算系统方差——迭代。
迭代，迭代，迭代
我们能做的就是在系统运行开始后计算每一次的理论值方差，根据这个方差和传感器方差计算出融合权重，每次都要重新计算————迭代。
计算,
按上文，每进行一次观测，就需要对两个分布的方差重新计算（一般传感器的方差不重新计算，设为定值），迭代下去，为了表示的更加清晰，下面用替换，替换
因为是迭代的，考虑第k个时刻，当前的最优估计值应该由根据之前时刻状态由运动模型计算出来的理论值，和时刻的传感器测量值，融合计算出时刻的kalman预测值。
所以把上述, 变为,代表由第t-1时刻的状态计算的x的理论值的方差，和t时刻的测量值的方差。
“概念释明: 之后所有公式中的代表kalman理论最终预测值,x代表由运动模型,即状态方程更新得到的值，x 后文会以理论值代称，后文会以预测值代称.
”
传感器测量值的分布一般可以当做已知(传感器噪声一般都是符合某个均值为0的固定分布的)，
因此只需计算 。
状态更新方程
当我们得到t时刻的kalman估计值，我们需要引入状态更新方程，得到新的理论值,同时计算理论值的方差。
“这里尤其要注意，状态更新每次都需要上一时刻的真实状态作为输入, 而我们无法获得真实的输入,只能拿上一时刻由kalman理论预测出来的作为输入计算理论值。
”
至此我们计算出了,（其中是根据先验确定的。），则就有了下面四个公式：
上面的公式从上到以下依次编号为(1)-(4).
上面这个公式才应该是最直观的协方差更新公式，但是我们常见的往往是下式推导得到的更简洁的式子。
然后把公式（3）代入上式，得到公式(5)，如下：
“上式将α改为k，以符合kalman理论的通用符号表示，下同。
”
公式（3） 就是1维的KALMAN增益计算公式，所谓KALMAN增益K就是有多相信当前的测量值。
公式（4）中的代表kalman预测值。
观测矩阵H
同时，实际模型中的测量值一般不是状态变量，二者存在关系：
这里的代表真实值，但是真实值是得不到的，上式的目的主要是为了得到,因为往往观测值和状态变量维度不一致，或者量纲不一样，所以需要这个h使得二者可以相加.
至此，我们得到了KALMAN滤波器的所有五个方程，即上述标号（1）-（5）的公式，其中 (1)为状态外推公式，（2）为协方差外推公式，（3）为kalman 增益计算公式，（4）为融合公式（有的也叫状态更新公式），（5）为协方差更新公式.
拓展到多维——就是用协方差矩阵 P 替换σ
简而言之，协方差矩阵就是多维情况下的方差。直接用P替换σ即可（H为放方阵时，不为方阵的特殊情况见后文）。
结束了吗？
没有,推导从不代表问题被解决，我一直贯彻的想法就是把公式化为直觉，才算真正学到了。
关于kalman滤波器的学习，我一直是断断续续，好似明白了，等过一阵子，在拾起来却又发现某个地方之前是完全没有理解到位的，依稀记得大二第一次接触kalman滤波器时，我抱着一本从学校图书馆借来的《kalman滤波器原理与matlab设计》，在回家的绿皮火车上看了几十页，似懂非懂，浑浑噩噩，再到后来，因为做小车要用到，便将网上流传甚广的两元kalman滤波器代码down了下来，放在了小车直立平衡环的角速度滤波上，当时只觉得kalman好像确实比一阶互补滤波要更稳，但是小车需要晃一段时间才能逐渐收敛到稳定的状态，那是我第一次对kalman的迭代性质有一个切实的认识。
现在想起来，当时我对kalman的似懂非懂就是卡在了协方差外推公式中，为什么要左乘一A矩阵右乘A矩阵的转置上，现在想起来真是由于当初自己的线代知识不够牢固而导致的贻笑大方的问题。
后来，就是研究生阶段再读kalman滤波器，便有了此文，也是修修改改反反复复多次，终于自认是推导明白了，但过程中也是出现了较多曲折，走进过一些误区，分享出来，警示一下大家。
kalman滤波器是具有markov性质的，但是其是综合了所有历史信息的，因此不是简单的从上一时刻预测下一时刻
kalman滤波器更像是一个做融合问题的方法，而不是预测，其对传感器有一定要求，传感器的误差均值应为0，而预测类问题往往是无法对系统有一个准确观测
除了状态外推方程外，kalman滤波器的变量就只剩Q,R了，其中Q代表的是系统预测的不确定性，R代表的是传感器的不确定性
直观理解kalman
kalman滤波器的核心就是加权融合，如果将状态更新方程写成连续形式,设 Δt=1
实际上，我们用kalman滤波器，可以理解成要把传感器当作输入，预测状态x的，，因此kalman滤波器就相当于一阶惯性环节。a就是kalman增益k，a越大，时间常数T越小，系统的惯性越小，传感器测量值反映到x的变化上更快，即越应该相信传感器，这和之前k的定义是一致的。
而kalman滤波器相当于实时的计算时间常数。
现在在看来kalman增益计算公式，对于1维情况（就是一开始提到的例子，如果用多维公式表示，可以看作A=1,H=1），公式3带入公式5
上面好像电阻并联的等效电阻计算公式，没增加一次观测，就相当于并联了一个 .
Q,R 的影响
下面的实验也是1维的，假设A=1,H=1,真实x=-0.37，初值为0.
增大R
R越小，会相对快速的相信传感器的值，所以会比较快的收敛的真实值附近。上图可以看到，随着R增大，收敛的越慢，系统惯性变大了，因为一开始系统初值为0 ，随意会很缓慢的到真实值附近。
但是随着R越大，并联后的阻值开始的时候就相对越大，即系统方差一开始也会较大。如下图
而根据前面的结论，如果系统方差较大，那么也会更相信传感器。总结一下，R变大了（要减小对传感器的信赖），导致滤波初期Q也会相对变大，初期Q变大了，应该会导致更相信传感器，这不是矛盾了吗？但是其实没有。千万不要陷入这样的误区，要记住P增大是R增大带来的影响，R是自变量，是主要原因，并且是先变化的，即使R导致初始Q变大了，也只是副作用而已，其影响不会大过R带来的。
增大Q
总结一下，有以下结论：
增大R，即传感器测量方差，则系统惯性大
增大Q，即系统理论方差，则系统惯性小
kalman增益的变化
kalman增益会慢慢收敛到某个固定值，这也是互补滤波和kalman效果一样的原因。而k值的变化也会受P，Q，R的初值影响，不过一般而言都会随着迭代次数增加而慢慢收敛。
P的初值，状态的初值的影响
实际工程中,不光要合理设置Q,R，也要合理设置P，和X的初值，其中Q,R的相对值更加重要，而且数量级要正确，只要数量级正确，都会慢慢收敛的，这一部分本文暂且不表。
kalman总结：
从上面的实验可以大致看出，kalman需要保证观测值分布必须保证0均值分布，不能有固定误差，否则一定会带偏估计值，同时kalman初期是需要时间收敛的，收敛的快慢取决于参数的整定，这也解释了我大二做车时的疑惑，当初进行角速度滤波时，kalman方法会导致小车一开始摇摆的相对剧烈，而一阶互补滤波就不会有这种问题。但是后期kalman增益收敛了，就会稳定。
贝叶斯滤波
挖坑待填。。。。
二维系统
我们假设Q,R是恒定的，如果还是以匀速运动模型为例（假设状态变量有两维，位置和速,且我们只能观测到位置）则有:
A,H代入上述公式，有
前面1维的时候说了主要是为了得到，但如果H不可逆怎么办，上面的推导也说了这个问题，但实际上这里的H逆是不需要求的，1维情况的例子是为了从kg引出Kg,但是我们直接算Kg的话只是少乘了一个H。H的作用可理解成x又经历了一个过程H,所以kalman增益更新的时候，P矩阵左右 乘以了H。
但是融合的时候就会发现，此时速度值的更新完全就是在前一时刻的基础上加了位置的更新量乘以一个较小的系数，也就是如果观测位移大于理论位移了，就会给速度更新一个正的补偿，小了给负的——其实也好理解，看到的位移比预测的大了，那可能速度应该比一开始理论的要大。这里发现kalman滤波器对于不客观量也有一定的合理估计，而这是由协方差矩阵导致的，因为速度确实会影响位置，那么二者肯定会存在一定联系。
有的时候也往往写成下式：
因为协方差矩阵是对称的，所以写成上式不影响最终结果。
补充：
笔者特地翻看了opencv kalman filter的源码，多维变量公式（3）中计算kalman增益时，分母求逆的计算是用最小二乘法得到的，这样可以保证分母出现不可逆的情况时也能用违逆代替正常计算。
其次，使用opencv 或者pykalman 进行滤波时，Q的维数是和状态变量保持一致的，R时和观测变量保持一致的。
二阶系统Q,R的初值怎么给？Q,R的值可以很大吗？初始值对系统的影响如何？运动模型建立错误对结果的影响如何？
笔者做了一个简单的实验，有兴趣的可以转战github看一看 github.com/Karmaadonis/kalman_filter/blob/main/kalm
① 全网独家视频课程
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求职交流等方向。扫码添加汽车人助理微信邀请入群，备注：学校/公司+方向+昵称（快速入群方式）
④【自动驾驶之心】平台矩阵，欢迎联系我们！
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
