1. 五个公式倒背如流自瞄该抖还是抖每年招新季队里几乎都会上演同一幕新人把学长留下的自瞄工程 clone 下来翻到KalmanFilter.cpp五个公式抄得一字不差参数照着某篇教程填上Q 0.001、R 0.1编译烧录上电云台一顿猛抖装甲板在视野边缘来回横跳。然后他跑来问这滤波器是不是有问题我一般会让他先把滤波关掉把原始数据打出来看一眼。十次里有九次问题不在滤波本身而在于他根本不知道自己喂进去的那串数据方差多大、噪声服从什么分布、两个状态量之间有没有相关性。卡尔曼滤波本质是一个用概率统计语言写出来的最优估计器。你把Q和R当成两个可以随便猜的玄学旋钮它当然只能用玄学的方式回敬你。在 RoboMaster 电控这条线上卡尔曼滤波露脸的地方比很多人想象得多自瞄预测敌方运动要用它IMU 和视觉融合要用它电机转速和电流环的观测要用它连超级电容的能量状态估算里都能见到它的影子。但它从来不是孤零零存在的它前面永远站着一排概率统计的基础概念。这一篇是电控合集里的前置篇专门把这块地基补上。不讲花哨的推导技巧只讲你在赛场上真正会用、必须搞懂的那部分概率统计。看完之后你至少应该能做到三件事拿到一段传感器数据能判断它的均值和方差的物理含义看到那五个公式能说清楚每个矩阵代表的是哪一部分不确定性调参的时候知道该动哪里、为什么动。如果你现在还在抄参数—跑不动—再抄一组的循环里这篇就是给你写的。2. 从陀螺仪那段波形说起随机变量到底描述什么2.1 单次采样是确定的一串采样才谈得上随机示波器上陀螺仪某个瞬间输出12.3 deg/s这是一个确定的数字谈不上随机。随机变量描述的是还没发生、或者你不知道结果的那个量。你在采样之前问下一次读数是多少这才是随机变量采完之后读出具体数值它就变成了这个变量的一次实现。RM 里最典型的场景是估计敌方装甲板下一帧的位置。它当前的准确位置你都不知道视觉本身有噪声它下一帧往哪走你更不知道对手在动。这个下一帧位置就是一个随机变量。我们做的所有滤波本质都是先给它一个概率分布再用一个点估计去替代整个分布。这个观念一定要摆正滤波器的输出从来不是真值而是在已有信息下最合理的猜测。很多新人把卡尔曼输出当成真值然后拿它和下一帧观测硬比发现对不上就怀疑滤波器坏了。其实是你拿了一个估计值去和一次带噪声的采样较劲本身就不公平。2.2 离散和连续两套语言不能混着用编码器读出来的是整数计数值属于离散随机变量它的概率用分布列来描述求和。IMU 输出电压换算出的角速度是连续的用概率密度函数来描述求积分。卡尔曼滤波推导里那些求和变积分的操作就是这套语言在起作用。你不需要每次都用积分去算但要知道自己处理的对象是哪一类。这里有个很实用的细节编码器量化本身就是一种噪声。假设编码器每圈 8192 线那么一个计数对应的角度步长 Δ 就是 2π/8192 rad。在均匀量化假设下量化噪声的方差近似为 Δ²/12。这个公式不是用来考试背的是用来告诉你一件事——量化的分辨率越低这个观测噪声越大你在卡尔曼里填的R就越大。很多车用低线数编码器做低转速观测时结果很差原因就在这里跟你滤波写得好不好关系不大。3. 期望、方差、协方差卡尔曼真正在算的三块砖3.1 均值滤波为什么能降噪期望给出了答案期望的离散写法是 Σ x · p(x)连续写法是 ∫ x · f(x) dx。它描述的是这个随机变量长期平均下来会落在哪里。我们常用的滑动平均滤波本质上就是用样本均值去估计期望。为什么平均能降噪假设每次测量都等于真值加上一个独立的、均值为零的噪声那么平均 N 次之后噪声的方差会变成原来的 1/N。这就是最朴素的降噪原理。但滑动平均有两个绕不开的毛病一是它假设噪声独立同分布传感器一旦有零偏就失效二是它会引入相位滞后对动态系统不友好。卡尔曼滤波比它强的地方是它会根据当前预测和观测各自的不确定性,动态地给两者分配权重而不是简单地平均。3.2 方差和标准差先把单位这件事搞明白方差定义为 Var(X) E[(X − E[X])²]标准差是它的平方根。在 RM 里陀螺仪静止时输出的波动程度方差就代表噪声功率。我做过一个很简单的实验云台静止不动采样 1000 个点算出来角速度标准差是 0.05 deg/s那么你的观测噪声R至少不应该比 0.05² 0.0025 小太多。这里有个高频踩坑点方差是平方单位。角速度的单位是 deg/s它的方差单位是 (deg/s)²。相当多的人把标准差当方差填进R结果量级直接差了一个平方。滤波看起来能用但增益一直算错动态响应要么迟钝要么发疯。填参数之前先在心里过一遍单位。3.3 协方差和协方差矩阵描述变量之间的关系协方差 Cov(X, Y) E[(X − E[X])(Y − E[Y])]它衡量两个变量一起偏离各自均值的倾向。正值表示一个变大时另一个倾向变大负值表示反向接近零表示线性上关系很弱。对卡尔曼更有意义的是协方差矩阵。假设状态是 [位置 x, 速度 v]那么协方差矩阵就是一个 2×2 的矩阵对角线是各自的方差非对角线是两者的协方差。为什么要有非对角线元素因为位置和速度往往不是独立的如果你这次测到的位置偏高很可能是因为这段时间整体速度偏快那么位置和速度的估计误差就是相关的。忽略这种相关性滤波器对不确定性的估计就会失真进而影响增益。很多人写一维卡尔曼觉得很简单一扩到二维就崩问题往往出在协方差矩阵的非对角元没有正确建模。概念符号物理含义在卡尔曼里的角色期望μ估计值大概落在哪状态量的点估计方差σ²这个估计有多不确定协方差矩阵的对角线协方差σ_xy两个量误差是否同步协方差矩阵的非对角元协方差矩阵P整个状态的不确定性形状每个循环都在传播和更新4. 为什么偏偏是高斯分布4.1 中心极限定理噪声为什么会长成高斯中心极限定理说的是大量独立同分布的随机变量相加其和的分布会趋近高斯分布。传感器噪声通常是很多微小误差源叠加出来的结果——热噪声、量化误差、电路干扰、机械振动来自不同地方、彼此近似独立。它们叠加在一起就长得像高斯。这就是卡尔曼滤波默认噪声服从高斯的现实依据不是什么强制假设而是一种在大多数情况下够用且方便的工程近似。要注意够用不代表永远对。如果某个传感器有严重的偏置漂移或者偶尔出现野值比如视觉误识别那它就不高斯的。这时候你需要先处理异常值而不是指望卡尔曼硬扛。我见过自瞄偶尔跳到错误的装甲板上滤波跟着一起偏最后整辆车方向都错了本质就是观测分布出了问题。4.2 高斯只需要两个参数,信念就能被完整描述一维高斯写成 N(μ, σ²)多维高斯写成 N(μ, Σ)。它的妙处在于只有两个参数而卡尔曼滤波的状态恰好就是这两样均值向量是状态估计协方差矩阵是这次估计的不确定性。所谓信念在卡尔曼里就是一个高斯。这也解释了一个常见误解有人以为卡尔曼输出的就是真值。不是它输出的是这个高斯分布的均值而高斯本身还带着一个协方差告诉你这个均值有多可信。忽略协方差只看均值就丢掉了一半信息。4.3 高斯经过线性变换还是高斯这是卡尔曼能闭合的关键如果 X ~ N(μ, Σ)而 Y A X b那么 Y ~ N(Aμ b, A Σ Aᵀ)。这条性质非常关键。它意味着只要你的运动模型和观测模型都是线性的并且噪声是高斯的那么预测出来的分布还是高斯更新之后的分布也还是高斯。整个递归过程永远是高斯进、高斯出不需要存储什么复杂的分布只要维护均值和协方差两个量就够了。协方差传播公式P_pred F P Fᵀ Q就是从这里来的。F是状态转移矩阵它把上一时刻的不确定性映射到当前时刻Q是过程噪声代表模型本身的不精确。如果模型是非线性的这条性质就破坏了你才需要扩展卡尔曼或者无迹卡尔曼去处理。这也顺带回答了一个问题什么时候可以放心用线性卡尔曼答当你在关心的工作范围内模型接近线性、噪声接近高斯的时候。5. 贝叶斯卡尔曼的骨架其实是个递归贝叶斯滤波器5.1 条件概率和贝叶斯公式条件概率 P(A|B) 表示在 B 发生的条件下 A 发生的概率。贝叶斯公式把它翻了个面P(A|B) P(B|A) P(A) / P(B)。用文字说就是后验概率正比于似然乘以先验。观测告诉你哪个状态更可能产生我看到的数据似然你先有的认知告诉你哪个状态本来就比较可能出现先验两者相乘再归一化就得到更新后的认知后验。卡尔曼滤波的更新步骤干的完全就是这件事。先验就是预测出来的那个高斯分布似然就是观测模型给出的分布后验就是两者融合的结果。之所以能用几个矩阵乘法直接算出来是因为高斯乘高斯还是高斯结果可以解析求出。5.2 预测和更新两步走背后的概率意义预测步骤拿运动模型把上一时刻的信念往前推一步。状态均值往前推不确定性会因为加了过程噪声Q而变大。这一步在问如果只相信我自己现在状态应该在哪有多不确定更新步骤拿新的观测来修正。观测越准不确定性越小。这一步在问现在我看到了新数据它告诉我状态应该在哪有多确定然后两者按各自的不确定性加权融合不确定性变小。一增一减反复循环滤波就活起来了。卡尔曼增益K P_pred Hᵀ (H P_pred Hᵀ R)⁻¹本质上就是一个权重它衡量的是预测的不确定性占预测不确定性加观测不确定性的比例。预测越不确定、观测越可信K越接近 1就更相信观测反过来K越接近 0就更相信预测。5.3 最小均方误差为什么均值就是最优估计有了后验分布之后你还要把它变成一个具体数字交给控制环。怎么选在均方误差最小的意义下最优估计就是后验分布的均值。而在高斯分布里均值、众数、中位数是同一个点所以取均值既是最优、又最自然。这就是卡尔曼输出那个数的来历——它不是随便挑的是在最小化估计误差平方这个准则下的最优解。理解这一点你也就明白了为什么卡尔曼在噪声非高斯时性能会下降那个漂亮的解析解依赖于高斯假设一旦假设不成立最优估计就不再等于后验均值了你得到的只是一个还在工作但不再最优的近似。6. 协方差矩阵最容易被忽略但其实最核心的主角6.1 P 矩阵到底在表示什么如果让我一句话概括卡尔曼滤波和普通滤波的区别我会说普通滤波维护一个数卡尔曼维护一个数加一个不确定性。那个不确定性就是P。它不是一个凭空出现的中间变量而是上一个循环算出来的结果会随预测和观测不断变化。P的结构很有信息量。对角线上是各个状态分量的方差值大说明这个分量估计得不准非对角元是状态之间的协方差反映了误差的相关结构。举个 RM 里的例子如果你的状态是 [俯仰角, 俯仰角速度]这两个量通常强相关——角度估计偏高时往往角速度估计也偏高。正确建模这种相关性能让滤波器在观测缺失时依然做出合理的推断。6.2 Q 和 R一个管模型一个管观测Q是过程噪声协方差代表你的运动模型有多不靠谱。R是观测噪声协方差代表你的传感器有多不靠谱。这两个矩阵决定了滤波器到底更信模型还是更信观测。参数调大的效果调小的效果起步建议Q更信观测响应快但易受噪声干扰更信模型平滑但滞后、跟不上机动从实测模型误差方差起步R更信模型输出平滑但对真实变化迟钝更信观测跟随快但对噪声敏感从静止采样方差起步P0收敛慢收敛快但初期可能过冲适当取大给滤波器重新学习的空间调参的思路其实就一句话你更相信谁就把谁对应的噪声调小。注意是相互的动一个都会改变两者相对关系所以每调一次都要观察输出再决定下一步。6.3 从一维到多维别在维度上翻车一维卡尔曼里P、Q、R都是标量写起来很清爽。扩到多维之后全变成矩阵。新手最容易翻的车包括矩阵维度对不上、转置写反、把R和Q填反。我建议一开始别急着上高维先拿一维标量版本把预测—更新两步骤走通确认输出符合直觉再逐维往上加。每加一维都要问自己这个状态量的物理意义是什么它和已有状态量有没有相关性它的观测噪声大概多大把这三个问题答清楚再去写代码比抄一份高维代码省心得多。7. 落到 RM 电控上的几条实在建议7.1 先看数据再谈滤波我强烈建议任何滤波上线之前先把原始数据打出来看。做法很简单让车静止采集几秒的陀螺仪或视觉数据算均值、方差画个直方图。均值可以看出有没有零偏方差给你填R的量级直方图形状告诉你接近不接近高斯。这三件事花不了半小时能省掉后面很多无头苍蝇式的调参。很多队伍的滤波器问题根本原因就是没人看过原始数据长什么样。7.2 参数初值怎么定给你一个能用的套路R从静止采样的方差起步Q从你模型预测误差的方差起步P0取一个偏大的值。这三条不是空话。比如你要融合陀螺仪和视觉来估计云台角度那R就从视觉静止时读数的波动方差起步Q就从陀螺仪积分一小段时间后的漂移量估算。先有一套有物理依据的初值再在这附近做小幅搜索效率远高于从零乱猜。调参时一次只动一个量观察响应变化用表格记下来。7.3 别一上来就上扩展卡尔曼EKF、UKF 这些听着高级但它们解决的问题是非线性和强非线性。RM 里绝大多数场景——转速估计、姿态融合、简单运动预测——线性卡尔曼都够用。我见过不少新人因为模型里有一个sin或者一个乘法就直接切 EKF结果雅可比矩阵推错、实现更复杂、调试更难性能反而不如老老实实做线性近似。记住一句能用线性近似搞定的就别加复杂度。什么时候真的需要 EKF当你发现线性化误差已经影响到最终控制效果、并且你能明确定位误差来源的时候。8. 几个我踩过、也看别人踩过的坑第一个坑是单位。前面强调过还是有人把标准差当方差。第二个坑是采样时间。卡尔曼里的dt必须和实际循环周期严格一致你用 1kHz 的循环但dt填了 1ms 以上模型就偏了调参怎么都调不好。第三个坑是坐标系。观测和状态不在同一个坐标系里你融合的是两个不同物理意义的东西输出必然离谱。第四个坑是延迟。视觉给的位置往往有几十毫秒延迟直接拿去更新模型在当前时刻的状态会引入系统偏差正确做法是先把观测对齐到对应的时间。还有一个特别隐蔽的坑把预测值和观测值混为一谈。预测是模型推出来的观测是传感器给的两者在更新公式里角色不同。有人在代码里让观测直接覆盖状态看起来响应很快其实已经退化成纯跟随滤波的平滑和抗噪能力全丢了。概率统计这块地基看着枯燥但它是你后面所有状态估计工作的支撑。我个人的体会是真正让卡尔曼调得顺的人往往不是公式记得最熟的那个而是最愿意花时间去理解数据、理解不确定性的那个。把这篇里的均值和方差搞扎实把协方差矩阵当成朋友而不是负担下一步再把五个公式和它们背后每一步的概率含义对上号你会发现那些曾经玄学的参数慢慢都有了物理意义。