021、扩展卡尔曼滤波（EKF）原理

飞控算法从入门到精通 · 021 · 扩展卡尔曼滤波（EKF）原理从一次炸机事故说起去年夏天，我在调试一架四轴无人机时遇到了一个诡异的问题：GPS信号良好，IMU数据看起来也正常，但飞控的姿态估计在急转弯时突然发散，飞机直接翻了个跟头砸在地上。事后回传日志发现，标准卡尔曼滤波（KF）在非线性运动下，状态预测和观测更新之间的协方差矩阵完全失配——简单说，就是滤波器“以为”自己很确定，但实际上已经偏到姥姥家了。那次之后我彻底明白：对于飞控这种强非线性系统（比如旋转矩阵的三角函数、空气动力学的二次项），线性卡尔曼滤波就是一把钝刀。而扩展卡尔曼滤波（EKF），正是给这把刀开了刃。为什么线性KF在飞控里不够用标准KF的核心假设是：系统状态转移和观测模型都是线性的。但在飞控中，我们面对的是这样的场景：状态方程：x_k = f(x_{k-1}, u_k) + w_k，其中f包含旋转矩阵、四元数乘法、加速度计模型等非线性函数观测方程：z_k = h(x_k) + v_k，比如磁力计测量地磁场方向，是状态的非线性函数如果你强行用线性KF，相当于把f和h当作线性函数处理。在姿态角变化小于5度时还能凑合，一旦遇到急转弯、剧烈加减速，泰勒展开的高阶项就会像滚雪球一样积累误差。我那次炸机，就是因为急转弯时旋转矩阵的线性近似误差超过了阈值，滤波器直接“失锁”。