卡尔曼滤波在RM电控里几乎是个绕不开的话题但凡做过云台、做过底盘跟随、做过视觉自瞄的队伍最后都会和它打交道。我当年第一次在电控代码里见到卡尔曼滤波是接手队伍云台控制模块时前人的代码里有一段五六个矩阵乘法的函数注释只有一行卡尔曼滤波别动。那时候我完全不明白那几个矩阵在干什么只知道调参数能让云台变稳或者变飘。后来踩了不少坑才慢慢明白卡尔曼滤波不是什么黑魔法它本质上就是一套根据预测和实测加权修正的框架只不过这个加权方式有严格的数学推导做支撑。这篇就结合RM电控的实际场景把卡尔曼滤波的数学思想、代码落地和调参经验完整拆一遍适合正在做电控、被传感器噪声和目标预测折磨、又不甘心只会调参的队员参考。1. 为什么RM电控绕不开卡尔曼滤波1.1 云台抖动背后的传感器噪声真相先说说为什么大家最后都会用上它。RM的云台要稳第一位的输入是姿态。而姿态来自IMU通常是陀螺仪加加速度计的组合。问题在于这两个传感器单拿出来都不够用。陀螺仪测的是角速度你对它做积分就能得到角度。听起来很美好但陀螺仪存在零偏也就是静止时输出的不是零而是某个小偏差。这个偏差哪怕只有0.1度每秒积分一分钟就是6度的漂移。比赛打三分钟云台可能已经歪了十几度这在自瞄场景里直接意味着脱靶。加速度计测的是重力方向静止时它能告诉你绝对的水平姿态没有累积误差。但它对振动极其敏感。RM赛场上底盘过坎、云台急停、摩擦轮启动都会带来剧烈的线性加速度加速度计分不清这是重力分量还是这是运动加速度算出来的角度瞬间就会跳变。如果你直接拿加速度计算的角度去控制云台云台会疯了一样抖。所以标准做法是把两者融合用陀螺仪积分提供高频响应用加速度计提供低频绝对参考把它们各自的漂移和噪声互补掉。这个过程就是卡尔曼滤波最经典的用法之一。RM电控的第一道坎往往就是我的姿态能不能在高速运动和强振动下依然稳。1.2 卡尔曼滤波到底解决了什么问题很多人把卡尔曼滤波理解成一种滤波算法和低通滤波、滑动平均放在一起比较。这个理解其实不准确。低通滤波只做一件事把高频成分滤掉代价是带来相位延迟。你滤得越狠云台响应越慢操作手感越拖沓。卡尔曼滤波的思路完全不同。它不是单纯地砍掉高频而是在做状态估计。也就是说它内部维护一个系统现在处于什么状态的信念用系统的物理模型去预测下一时刻的状态再用传感器实测去修正这个预测。预测和修正谁更可信取决于各自的不确定度也就是协方差。不确定度大的那一方权重就低。这个区别在RM里特别关键。云台在高速转动时我希望滤波能快速跟上真实角速度不希望被过度平滑云台在静止微调时我又希望抖动能被压下去。传统的固定低通做不到随场景自适应而卡尔曼滤波因为内部有协方差在动态调整天然就具备这种该快就快、该稳就稳的能力。提示判断你的滤波是不是卡尔曼看它有没有预测这一步。只对测量值做加权处理的那叫互补滤波或者滑动平均不是卡尔曼。RK的RM电控教程里之所以反复强调卡尔曼滤波是因为它同时覆盖了姿态解算、目标预测、速度估计三个大类需求。你学会一套框架能解决一大半传感器相关的电控问题。这也是它性价比高的原因。2. 从五个公式看懂卡尔曼滤波的数学骨架2.1 状态空间模型是整个算法的地基卡尔曼滤波的第一步永远是建立状态空间模型。所谓状态就是你想估计的那些量。做姿态解算时状态可能是角度和陀螺仪零偏做云台目标预测时状态可能是目标的角度和角速度做底盘速度估计时状态可能是速度。状态怎么演化到下一时刻靠状态转移方程。最朴素的写法是新角度等于旧角度加上角速度乘以采样周期。这一句话翻译成矩阵就是状态转移矩阵F。它是整个滤波的骨架决定了你的模型对物理世界的认知有多准。RM里常见的一个坑是很多新手直接把状态定义成角度忽略了角速度然后发现滤波器对快速运动的响应很差。因为只估计角度滤波器不知道该往哪个方向变化多快。把角速度也纳入状态滤波器就能通过观测到的角度变化率间接推算出角速度这在云台前馈控制里非常有用。2.2 预测与更新两步循环的物理含义卡尔曼滤波的核心是一个循环每来一个采样周期就跑一次分两步。第一步是预测也就是根据我上一时刻的估计我猜现在状态是什么。这一步做两件事一是用状态转移矩阵把状态推进到当前时刻二是把协方差也推进同时加上过程噪声Q。协方差代表我对这个估计有多不确定每预测一次不确定性会因为模型误差而变大这就是加上Q的原因。第二步是更新也就是我拿传感器实测来修正刚才的预测。这里会先计算卡尔曼增益KK的本质是一个权重如果测量噪声R很大传感器很烂K就小我就更信预测如果预测协方差很大我对模型没底K就大我就更信测量。算完增益后用实测值减去预测的测量值得到新息再乘以增益去修正状态最后修正协方差。整个流程就是预测、更新、预测、更新周而复始。理解了这两步的物理含义那些矩阵公式就不再是天书而是一个很自然的先猜后校的过程。2.3 卡尔曼增益在做什么样的权衡单独把卡尔曼增益K拎出来讲是因为它是理解卡尔曼滤波聪明之处的关键。K的公式本质上是预测不确定度除以预测不确定度加测量不确定度。假设预测协方差是P测量噪声是R那么K约等于P/(PR)。你可以把它理解成最终估计值等于预测值和测量值的加权平均权重由各自的不确定度决定。这个结构非常优雅。当系统刚启动我对状态一无所知P很大K接近1几乎完全信任测量快速收敛当系统稳定运行P变小K也变小滤波变得更平滑抑制测量噪声。也就是说这个权重是自动变化的不需要你手动去调一个固定的滤波系数。注意很多人以为自己在调卡尔曼滤波的参数其实大部分时候是在调Q和R这两个噪声矩阵。理解K的动态含义才知道调Q和R会影响什么。3. RM电控中的三大落地场景3.1 IMU姿态解算加速度计和陀螺仪如何互补这是最经典的场景。状态取两个量角度和陀螺仪零偏。状态转移是角度加角速度乘周期零偏保持不变假设零偏缓变。观测量是加速度计算出来的角度。这里有个技巧很多人会额外把零偏也纳入状态去估计。好处是滤波器会自动估算出陀螺仪的漂移量并在积分时把它减掉。这样即使陀螺仪有温漂姿态也不会一直歪。代价是模型更复杂Q矩阵需要调得更细。实际写代码时要注意观测方程是非线性的加速度计算角度用了反正切严格来说应该用扩展卡尔曼滤波EKF需要在当前估计点对观测方程求雅可比矩阵做线性化。不过在RM这种小角度、姿态变化相对温和的场景下很多队伍直接用线性卡尔曼近似也能跑得不错如果发现大角度时精度下降再升级成EKF。另一个常见做法是把四元数作为状态避免欧拉角的万向锁问题。云台如果允许大范围俯仰欧拉角的奇异点会带来麻烦四元数加EKF是更稳妥的方案只是矩阵维度变成四维计算量上升。3.2 云台目标预测与延迟补偿自瞄系统里视觉给出目标位置是有延迟的一帧图像处理完可能已经过去十几毫秒。如果直接把这一帧的目标角度发给云台云台永远在追一个过去的目标高速横移场景下必然滞后。卡尔曼滤波在这里的用法是把目标的角度和角速度作为状态视觉观测作为测量跑一遍滤波。滤波输出的不仅