097、运动控制中的传感器融合：卡尔曼滤波基础

097 运动控制中的传感器融合：卡尔曼滤波基础

从一次电机堵转说起

去年调试一台四轮差速底盘，IMU和编码器数据打架打得厉害。电机堵转瞬间，编码器读数直接归零，但IMU的角速度积分还在往上窜——底盘明明卡在门槛上，导航算法却以为车在漂移。那会儿我盯着串口打印的波形，突然意识到：单一传感器在运动控制里就是个“偏执狂”，要么信得过但噪声大，要么精度高但会突然失明。

卡尔曼滤波不是万能的，但它是把传感器融合这件事从“拍脑袋加权”变成“数学最优估计”的起点。今天这篇笔记，咱们就把它从理论拽到嵌入式板子上。

状态估计的本质：你永远不知道真实值

运动控制里最折磨人的不是PID调参，而是“当前到底在哪”。编码器有累计误差，IMU有零偏漂移，激光雷达有遮挡——每个传感器都在撒谎，只是撒谎的方式不同。

卡尔曼滤波干的事很简单：用模型预测一个值，用测量修正这个值，然后根据谁更靠谱来决定信谁多一点。这个“谁更靠谱”就是协方差矩阵，也是整个算法里最容易写崩的地方。

举个实际例子：你让电机以1000rpm转，编码器反馈是998±5rpm，IMU解算出来是1005±20rpm。直觉上应该更信编码器，但电机突然堵转时编码器瞬间变成0，这时候IMU虽然不准但至少没跳崖——卡尔曼滤波就是那个能在两种极端情况间平滑切换的裁判。

线性卡尔曼滤波的五条公式（别背，要理解）

网上到处是那五条公式的推导，但嵌入式工程师不需要证明收敛性，需要知道哪条公式在什么情况下会炸