1. 先聊清楚学卡尔曼滤波为什么要补概率统计如果你是在校学生大概率和我当年一样第一次翻开卡尔曼滤波相关的资料看到的是一个状态方程、一个观测方程然后突然冒出来一大串带下标的矩阵紧接着就是协方差矩阵的递推更新。很多人在这一步就懵了——前几行还能看懂符号到后面完全不知道这些矩阵为什么是这样乘、为什么是这样更新。其实这种懵不是数学能力的问题而是知识结构缺了一块。卡尔曼滤波本质上是一套建立在概率统计框架之上的状态估计算法它的每一个关键步骤背后都有对应的概率统计语义。看不懂公式不在于矩阵运算不熟而在于对“概率分布是怎么描述一个不确定量的”“观测信息如何修正先验认知”这些底层概念没有形成直觉。这篇是中科大RM电控合集里卡尔曼滤波系列的前瞻篇目的很明确在正式开始推导卡尔曼滤波的五个核心公式之前先把概率统计基础打牢。这篇内容不直接给出卡尔曼滤波的完整推导但会把所有后续推导会用到的概率统计概念逐一拆开用RM电控场景下的例子来讲清楚。适合谁看两种人。第一种是完全没有概率统计基础的新队员刚进电控组被学长安排去了解一下卡尔曼滤波结果一搜资料全是数学公式。第二种是学过概率论但早就还给老师了的老队员脑子里大概记得“高斯分布”“期望方差”这些名词但遇到具体问题不知道它们怎么和滤波挂钩。这两种人都可以放心往下看。我会尽量不堆公式用比赛里真实遇到的场景来讲概念先把直觉建立起来。有了这个底子之后再上公式推导就不是硬啃数学而是顺理成章的事了。2. 把“状态估计”这个词翻译成人话2.1 机器人比赛里的“状态”到底是什么先说清楚我们处理的对象状态。在RM电控里状态这个概念到处都在用只是大家平时没有把它概括成术语。底盘的速度算不算状态算。云台当前指向的角度算不算状态算。发射机构的摩擦轮转速算不算状态算。视觉给出来的敌方装甲板位置算不算状态也算。本质上任何一个“用来描述系统当前情况的数值”都可以被称为状态。那“状态估计”是什么呢用一个最简单的情况来说明。假设你做一个单轴云台用的是编码器电机。编码器能直接读出电机转子的机械角度这个角度经过减速比换算就能得到云台的实际指向角。听起来好像不存在“估计”的问题——直接读编码器不就行了问题在于编码器读出来的数值是带噪声的。走线有干扰、ADC量化有误差、电机运转时的电磁干扰都会叠加在读数上。如果你在比赛场上云台高速扫动停下来之后面板上显示的角度和实际指向差了那么一点点这就是噪声造成的偏差。更麻烦的是如果你还要估计云台的角速度那就不能直接对角度做差分——差分会把噪声放大得一塌糊涂得到的数据根本没法用。这时候就需要状态估计了。所谓状态估计就是综合利用你手头所有带噪声的信息去推断系统真实状态的一个合理数值。它不追求每一次都百分百准确但追求在统计意义上尽可能接近真实值。2.2 一个“带噪声的测量”该怎么描述现在问题来了我知道编码器读数是带噪声的我也知道不能直接把读数当真实值那该怎么描述这个“带噪声的量”这就必须引入概率统计的语言了。在确定性的世界里一个量就是一个数5就是595就是95。但在带噪声的世界里一个量不再是一个确定的数而是一个“可能的取值范围”每个取值还有不同的“可信程度”。用生活化的类比来理解。你约了朋友在比赛场地门口碰头他告诉你“大概还有十分钟到”。这句话在你的预期里不是一个精确的“600秒”而是“大概率在8到12分钟之间但也不排除中间被导师叫住聊两句导致十五分钟才到”。也就是说你对朋友到达时间的认知不是一个确定值而是一整个分布有的时间点可能性高有的时间点可能性低。传感器读数也是这个道理。当你读到编码器返回的30.2度时你对真实状态的理解是真实角度大概在30.2度附近离这个值越近的概率越大离得越远的概率越小。把这个想法用数学写出来就是概率密度函数。所以你看概率统计不是一个和滤波无关的纯数学工具它本身就是描述“带噪声的测量结果”的天然框架。没有这个框架你只能拍脑袋定一个数有了这个框架你能精确地表达“我知道它大概在哪但不完全确定”这件事。2.3 卡尔曼滤波的本质信息融合等你接受了“传感器的读数是一个分布”这个想法之后卡尔曼滤波的核心思想其实已经呼之欲出了。在RM赛场上对同一个状态你通常有多个信息来源。云台角度编码器能测陀螺仪的积分也能算。机器人位置视觉能看轮式里程计也能推。问题来了这两个来源都有噪声而且噪声特性不一样——编码器短期准但容易被干扰带偏陀螺仪积分长期稳但会随时间漂移——你到底该信谁卡尔曼滤波给的答案是两个都信但是按“谁更可信就信谁多一点”的原则来加权融合。这里的“更可信”不是拍脑袋定的是用概率统计里的方差来描述的。方差小说明这个来源的不确定性低可以多信一点方差大说明这个来源比较飘要少信一点。这个思路说起来简单但它需要你对概率统计里的几个核心概念有相当扎实的理解随机变量、概率密度函数、期望、方差、协方差、高斯分布、贝叶斯定理。接下来的几个部分我会逐个拆开来讲每个都结合RM电控的具体场景来加深理解。3. 随机变量与概率密度传感器读数的“性格描述”3.1 随机变量不是一个数而是一种规则我见过不少同学对“随机变量”这个词有误解觉得“随机变量就是值不确定的变量”。这个理解不算错但过于模糊不太能支撑后续的理解。更准确地说随机变量是一个“取值规则”它描述的是一个试验的每一种可能结果对应哪个数值。掷骰子的结果是一个随机变量因为它把“出现1”“出现2”……“出现6”这六种可能对应到了1到6这六个数值上。传感器读到的噪声值也是一个随机变量因为它把“每一次测量中出现的随机干扰”对应到了一个实数值上。在状态估计里我们关心的是这样的问题如果我对真实状态做一个测量得到的读数会是多少由于噪声的随机性这个读数不是一个确定值而是一个随机变量。不同传感器的噪声特性不一样对应的随机变量也不一样——有的传感器读数总是在真实值附近小幅波动有的传感器读数偶尔会蹦出离谱的偏差。用RM里的场景来举例。同样是对云台角度的测量编码器读数的随机性和陀螺仪解算出来的角度的随机性就不一样。编码器的短期波动通常较小且集中而陀螺仪解算受积分漂移影响读数可能在短时间内很稳但时间长了会慢慢偏离真实值。这两者的随机性特征是不同的后面我们会用方差这个量来量化这种差异。3.2 概率密度函数像逐一记录弹道的靶纸有了随机变量下一步自然的问题是它的取值到底是怎么分布的哪些值容易出现哪些值不容易出现回答这个问题需要概率密度函数。我比较喜欢的类比是打靶。想象你在靶场上打了很多发子弹每一发子弹落到靶纸上就是一个弹孔。打完之后把靶纸拿下来看弹孔密集的地方说明子弹容易打到那里弹孔稀疏的地方说明不容易打到。概率密度函数就是这张“弹孔密集程度分布图”的数学版本。拿RM激光雷达的测距来举例。假设目标在3米外你拿着雷达测了一百次把每次测到的距离记下来画成直方图大概率会看到大部分读数集中在2.95到3.05米之间极少数的读数落在更远或更近的地方。这个直方图如果做得足够精细、测量次数足够多就会趋近于一条光滑的曲线这条曲线就是测距误差的“概率密度函数”。这里有个关键点需要强调概率密度函数上某一点的高度并不直接代表“取这个值的概率”。对于连续型随机变量取到某个精确值的概率实际上是0——这听起来有点反直觉但仔细想一想测距值恰好精确等于2.9837521米和恰好等于2.9837522米是两种不同的结果而每一种精确值的概率都趋近于0。真正有意义的是“落入某一段区间”的概率也就是概率密度函数在某一区间上的积分。不过在实际工程里我们不用抠这么细。你只需要建立这个直觉概率密度函数高的地方真实值大概率在那个附近概率密度函数低的地方真实值落在那里的可能性就小。3.3 状态估计视角下的概率密度把概率密度函数用到状态估计里就会得到一个非常重要的视角当我说“我对云台当前角度的估计是30.2度”时我其实不是在说“我觉得真实角度就是30.2度”而是在说“我对真实角度有一个完整的概率描述——它最可能落在30.2度附近离30.2度越远可能性越小”。这个视角的转变非常关键。在传统的控制思维里你会把估计值当成一个确定的数然后基于这个数去做控制。但在概率框架里你始终携带的是一个完整的分布卡尔曼滤波做的所有事情本质上就是在维护这个分布并随着新一轮的测量不断更新它。听起来很复杂所幸的是在卡尔曼滤波的场景里我们不需要面对任意的概率分布。我们处理的是一类非常特殊、性质非常好的分布——高斯分布。这也是为什么卡尔曼滤波可以有一套简洁漂亮的递推公式。下一部分我会专门讲高斯分布和它的两个关键参数。4. 高斯分布为什么卡尔曼滤波围着它转4.1 高斯分布的“形状”和它的参数高斯分布也叫正态分布是概率统计里最重要的一个分布。它的概率密度函数画出来就是那条中间高、两边低、左右对称的钟形曲线。高斯分布由两个参数完全决定均值和方差。均值决定了曲线最高峰所在的位置也就是分布的中心方差决定了曲线是“又高又瘦”还是“又矮又胖”。方差小说明分布集中随机变量的大多数取值都靠近均值对应传感器的测量比较稳定方差大说明分布分散取值东一个西一个对应传感器的测量比较飘。用RM的编码器来举例。如果编码器在静态情况下测一个固定的角度得到的噪声分布假设是均值为0、方差较小的——也就是读数大多落在真实值附近很小的范围内。而如果换成一个受电磁干扰严重的传感器它的噪声分布可能均值也是0但方差很大读数就会明显发散。这里有个细节需要留意均值和方差描绘的是分布的不同侧面。两个传感器可能均值相同都是“平均来看没偏”但方差不同一个稳定一个发散。在卡尔曼滤波里这两个参数都会被用到——均值告诉你“状态的估计值是什么”方差告诉你“这个估计值有多可信”。4.2 为什么自然界和工程里的噪声都倾向高斯你可能会有疑问为什么卡尔曼滤波非咬着高斯分布不放传感器噪声真的都是高斯分布吗严谨地说不是所有噪声都是高斯分布。但在绝大多数工程场景下高斯分布是一个非常好的近似而且有理论支撑。这就是中心极限定理大量相互独立的微小随机因素叠加在一起它们的总和会趋近于高斯分布无论这些因素本身服从什么分布。拿发射机构的测速来做例子。摩擦轮转速的测量误差可能来自编码器量化、轴承摩擦波动、电机电流纹波、信号线串扰等多个方面。每一个因素单独看都不是高斯的但当它们叠加在一起时总误差非常接近高斯分布。这就像你想预测一队队员从宿舍走到工位需要多长时间每个人出门时间、路上遇到什么人、电梯等多久都是独立的随机因素加起来之后总用时的分布会趋近于一条钟形曲线。这条性质让高斯分布成为状态估计里最“省心”的假设。你不用费劲去建模每一个误差来源的具体分布只需要用均值刻画系统性偏差、用方差刻画随机波动幅度就能得到一个足够好的模型。4.3 高斯分布的两个“好脾气”性质高斯分布之所以在卡尔曼滤波里扮演核心角色不仅仅是因为它常见更因为它有两个非常好的数学性质让递推计算成为可能。第一个性质是对高斯分布的随机变量做线性变换得到的仍然是高斯分布。这个很重要。在卡尔曼滤波里状态从上一时刻到下一时刻的演化通常建模为一个线性变换比如匀速运动模型里位置等于上一时刻位置加上速度乘以时间间隔。只要输入是高斯的输出也自动保持高斯的“身份”我们就可以继续用均值和方差两个参数来描述它。第二个性质是两个高斯分布的乘积经过归一化之后仍然是高斯分布。这个性质更关键它直接支撑了卡尔曼滤波里的“更新”步骤。前面说过卡尔曼滤波的核心是信息融合——把带噪声的测量和已有的估计融合在一起。在概率语言里融合就是“相乘”用测量信息修正已有的分布得到一个新的、不确定性更小的分布。高斯分布的这个“相乘封闭”性质保证了融合后的结果依然能用高斯分布描述从而让递推可以一直进行下去。这两个性质合在一起构成了卡尔曼滤波能够用有限个参数均值和方差完成无穷次递推的数学基础。如果换一个分布这个性质可能就不成立了运算复杂度会爆炸式增长。理解了这一点你就能明白为什么卡尔曼滤波的推导几乎全程都在和高斯分布打交道。5. 期望、方差与协方差卡尔曼公式里藏着的“直觉”5.1 期望把所有可能性加权平均讲完了分布开始讲分布的数字特征。期望是最基础的一个。期望的直观含义就是随机变量在所有可能取值上的“加权平均”权重就是概率。用掷骰子来说每个面出现的概率都是六分之一期望值就是(123456)/63.5。注意期望值本身不一定是随机变量能取到的值——骰子永远掷不出3.5但它代表了“如果掷很多次平均值会趋近于3.5”。在状态估计里期望的对应物非常明确它就是你对真实状态的“最佳猜测”。当卡尔曼滤波最终输出一组数值时这些数值其实是状态的后验分布的期望。这背后有一套完整的统计决策理论但通俗来说就是如果你只能给一个确定的数那么从平均意义上误差最小的选择就是分布的期望。所以当你看到卡尔曼滤波的递推公式里某个状态量的预测值是“上一时刻的估计值经过状态转移矩阵乘一下”你完全可以把它翻译成人话我在把上一时刻的最佳猜测按照系统的运动规律往前推一步。这里没有玄学全是直觉。5.2 方差可信度的量化标尺期望告诉你“猜什么”方差告诉你“猜得有多准”。方差的数学定义是随机变量与期望之差的平方的期望。它刻画的是随机变量取值的离散程度——取值离期望越远、越分散方差越大取值都紧紧贴着期望方差就小。方差更常用的是它的平方根标准差是卡尔曼滤波里最核心的“权重”指标。再来回顾一下思想实验编码器和陀螺仪解算都能给你云台角度怎么融合答案就是看谁的方差小。方差小的来源代表它的不确定性低就应该获得更高的融合权重。卡尔曼滤波的五条核心公式里有一个专门计算融合权重的式子那个权重本质上就是一个“按方差分配可信度”的系数。在实际调试RM电控时对方差参数过程噪声协方差和测量噪声协方差的调节是在调节什么往深了说就是在表达你对“运动模型”和“传感器测量”分别有多大的信任度。你如果把过程噪声调得很小卡尔曼滤波就会非常信任运动模型的预测结果就是滤波输出非常平滑但对传感器读数变化很不敏感出现“反应迟钝”的现象。反过来如果你把测量噪声调得很小滤波输出就会很贴近原始测量值响应快但噪声也大。理解了方差在融合里的作用调参就不是瞎试了而是有方向性的选择。5.3 协方差描述“两个量一起变”的程度方差描述的是单个随机变量的离散程度。但如果你的系统状态不止一个变量比如云台的状态同时包含角度和角速度你还需要知道这两个变量之间的关系——它们是一起变大、一起变小还是互不相干这就引出了协方差。协方差的直观含义是两个随机变量相对于各自期望的偏离是否倾向于同时出现。如果角度偏大的时候角速度也倾向于偏大这两个变量的协方差为正如果角度偏大的时候角速度倾向于偏小协方差为负如果它们之间没有稳定的联动关系协方差接近0。协方差在卡尔曼滤波里的角色容易被新手忽略但其实非常重要。还是拿云台的角度和角速度来说。真实物理世界里角度和角速度本身就是强相关的——角度持续增大角速度大概率就是正的。卡尔曼滤波在递推时如果忽略这个相关性只单独维护角度和角速度各自的方差就会丢失一部分信息而基于错误假设的融合结果也会次优。在卡尔曼滤波的完整公式里协方差是以矩阵的形式出现的也就是协方差矩阵。矩阵对角线上的元素是各个状态量自己的方差非对角线元素就是状态量之间的协方差。整个卡尔曼滤波的递推过程本质上就是在不断地更新这个协方差矩阵——预测阶段协方差会因为模型的不确定性而变大更新阶段协方差会因为测量信息的引入而缩小。5.4 独立性与不相关性什么时候可以简化说完协方差顺便提一下它有一个特例当两个变量的协方差为0时它们被称为不相关的。如果两个变量不仅不相关而且严格独立那么它们的联合分布可以分解为各自分布的乘积处理起来会简单很多。在实际建模中我们经常做这样的近似假设不同传感器之间的噪声是相互独立的。这个假设大多数情况下是合理的——编码器的噪声和陀螺仪的噪声来源是两套物理系统确实没有明显的联动关系。但要注意这只是一个工程近似不是绝对真理。比如共用一个电源的传感器在供电波动大的时候噪声就可能产生相关性。不过对卡尔曼滤波的基础应用来说通常可以放心地把不同传感器的测量噪声视为独立的这能让问题简化不少。6. 贝叶斯视角卡尔曼滤波的“魂”其实在这里6.1 先验、似然与后验一个侦探破案的类比卡尔曼滤波之所以强大是因为它背后有一个非常优雅的概率推断框架——贝叶斯定理。很多教材把贝叶斯定理放在概率统计的最前面讲但我觉得放在这里在你已经理解了分布、期望、方差之后再回头看它体会会深得多。贝叶斯定理的通俗版本是在看到新证据之前你心里有一个对某个量的初步判断先验然后你观察到了一些证据这些证据在不同假设下出现的可能性是不一样的似然把先验和似然结合起来你就得到了更新后的判断后验。用侦探破案来类比。一名侦探接到一个案子初步怀疑对象是一名惯偷因为这类案件惯偷作案的概率比较高——这是先验。但随后法医提供的证据表明案发现场留下的痕迹和另一个人的特征更吻合——这是似然。侦探综合考虑先验和证据最终把怀疑重点转移到了另一个人身上——这就是后验。在RM的状态估计里先验就是你对状态的既有判断。比如上一时刻我估计云台在30度按运动规律推算这一时刻应该到35度左右。这个“35度左右”就是先验。似然则是当前传感器测量提供的信息。如果编码器测出来当前角度是38度考虑到编码器有噪声我们得到的似然是真实角度在38度附近的可能性最高。后验就是综合“35度左右”和“38度附近”这两个信息之后得出的最终估计值。6.2 卡尔曼的“预测”就是在算先验“更新”就是在算后验把贝叶斯视角和卡尔曼滤波的步骤对应起来你会瞬间看穿整套算法的逻辑。卡尔曼滤波的典型流程分为两步。第一步叫预测用系统的运动模型把上一时刻的后验分布推导到当前时刻得到一个先验分布。这个分布的中心就是状态预测值它的不确定度协方差会相对上一时刻有所增大因为运动模型本身有噪声你不可能完美预测系统怎么走。第二步叫更新拿到当前时刻的传感器测量之后结合测量模型用贝叶斯定理把先验分布和似然分布融合起来得到后验分布。这个分布的不确定度会比先验和测量各自都小因为信息融合本质上是在“取长补短”。所以卡尔曼滤波的五个公式本质上就是贝叶斯定理在高斯分布和线性系统假设下的解析解。你不需要死记硬背那些公式只要记住一条主线用运动模型做预测得到先验用传感器测量做修正得到后验然后不断循环。其余所有的矩阵运算、协方差递推、增益计算都是这条主线的数学展开。6.3 一个递推的视角信息在流动而不是堆积贝叶斯视角还能帮你建立一个非常重要的认知卡尔曼滤波是递推的不需要保留历史数据。很多同学第一次接触滤波会下意识地往最小二乘拟合那个方向想——收集一堆历史数据然后拟合出一条曲线。但卡尔曼滤波不是这么做事的。它的美在于每一时刻输出的后验分布已经“压缩”了所有历史信息的精华。到下一时刻它只需要当前的先验分布和当前测量就能完成更新不需要把过去所有的测量翻出来重新算一遍。这意味着在嵌入式平台上的资源开销是固定的不随运行时间增长而变化。对RM这种资源受限的竞赛平台来说这个特性是极其宝贵的。你可以让滤波器在嵌入式系统里以固定周期一直跑下去内存占用恒定计算量恒定不会因为跑了几分钟就开始出现性能衰减。这也是卡尔曼滤波在嵌入式状态估计场景里碾压很多离线算法的重要原因。7. 线性系统的“状态空间”表达滤波要处理的载体7.1 从状态方程到状态空间模型讲完了概率基础还需要再把一个衔接性的内容铺垫好状态空间模型。卡尔曼滤波不是凭空作用的它需要一个具体的数学载体来描述系统——这就是状态空间模型。状态空间模型把系统的描述分成两个部分。第一部分是状态方程描述状态如何随时间演化。比如一个匀速运动的物体它的状态是位置x和速度v状态方程就是新位置旧位置速度x时间间隔速度保持不变。写成矩阵形式就是状态向量乘以一个状态转移矩阵再加上过程噪声。第二部分是观测方程描述传感器的测量值如何由状态计算出来。比如编码器测量的是云台角度θ那测量值就是θ加上测量噪声写成矩阵形式就是测量向量等于观测矩阵乘以状态向量再加上测量噪声。卡尔曼滤波的所有推导都建立在这两个方程之上。状态方程里的状态转移矩阵就是系统运动规律的数学化观测矩阵则决定了传感器的读数与状态之间的关系。你在调卡尔曼滤波的时候头