099、运动控制中的传感器融合：无迹卡尔曼滤波

099 运动控制中的传感器融合：无迹卡尔曼滤波一、从一次炸机事故说起去年夏天调试四足机器人，IMU和编码器数据打架，机器人原地转圈然后翻倒。示波器抓出来的波形让我头皮发麻——IMU的角速度积分已经漂了30度，而电机编码器反馈的位置还在原地踏步。当时用的扩展卡尔曼滤波（EKF），雅可比矩阵算得我手抖，结果还是炸了。后来换成无迹卡尔曼滤波（UKF），同样的传感器，同样的硬件，机器人稳稳当当走了两公里。不是EKF不好，是强非线性系统里，雅可比矩阵的线性近似就像用直尺量地球周长——误差会累积到让你怀疑人生。二、UKF的核心思想：用采样代替线性化EKF的问题在于，它把非线性函数强行掰直了再算。比如你的运动模型是x_next = f(x) + noise，EKF的做法是求f对x的偏导，得到一个线性近似。但遇到像四元数旋转、三角函数嵌套这种强非线性，线性近似直接崩盘。UKF的思路很暴力：我不去算导数，我直接撒一把点（Sigma点），让这些点穿过非线性函数，再从穿过后的点里统计出均值和方差。这就像你要知道一群羊过桥后的分布，EKF是画个桥的切线方程，UKF是直接赶几只羊过去看看。具体操作分三步：第一步：生成Sigma点假设当前状态是x，协方差是P。我们围绕x撒2n+1个点（n是状态维度），每个点代表一个可能的真实状态。这些点的位置由P的Cholesky分解决定，距离中心点的距离由参数λ控制（λ = α²(n+κ) - n，α通常取0.001，κ取0或3-n）。