098、运动控制中的传感器融合：扩展卡尔曼滤波

运动控制中的传感器融合：扩展卡尔曼滤波从一次电机堵转说起去年调试一台四轮差速底盘，IMU和编码器数据对不上。编码器说轮子转了10圈，IMU说车头偏了15度。现场工程师说“传感器坏了”，我说“算法没写好”。后来拆开看，轮子确实打滑了——编码器在空转，IMU才是真实的。这个场景让我意识到，运动控制里最坑的不是硬件故障，而是你信了不该信的传感器。扩展卡尔曼滤波（EKF）就是用来解决这个问题的：当多个传感器都在撒谎，你得知道谁在说真话，谁在说假话，以及怎么把假话修正回来。为什么标准卡尔曼不够用标准卡尔曼滤波假设系统是线性的。但运动控制里，哪有那么多线性？机器人转弯时，角度变化是三角函数关系；无人机姿态解算，四元数更新是非线性的。你硬要用线性模型去拟合，结果就是滤波器发散——我见过有人把卡尔曼增益算成NaN，因为协方差矩阵不满足正定性了。EKF的核心思想很简单：把非线性函数在当前状态估计值附近做一阶泰勒展开。说白了就是“用切线代替曲线”。代价是计算雅可比矩阵，但换来的是能处理大多数实际运动模型。EKF的五个步骤（别背公式，理解物理意义）第一步：状态预测假设你有一个两轮差速底盘，状态量是[x, y, theta, v, w]——位置、朝向、线速度、角速度。运动模型是：x_k = x_{k-1} + v * cos(theta) * dt y_k = y_{k-1} + v * sin