4、扩展卡尔曼滤波：非线性系统线性化、雅可比矩阵计算、EKF在组合导航中的应用

各位同学，大家好。今天我们聊一个在组合导航里绕不开的话题——扩展卡尔曼滤波，也就是EKF。

说实话，我刚入行那会儿，觉得卡尔曼滤波已经够用了。直到第一次在实车上跑惯导+GPS组合，发现车子一转弯，状态估计直接飞了。我当时就懵了。后来才明白，问题出在系统是非线性的，而标准卡尔曼滤波只适用于线性系统。

嗯，今天我们就来把这个坑填上。

4.1 为什么需要扩展卡尔曼滤波？

标准卡尔曼滤波有个前提：系统状态方程和观测方程都必须是线性的。但现实世界哪有那么听话？

你想想看，惯导里的姿态更新，涉及三角函数、四元数乘法，这哪是线性？GPS的伪距观测方程，包含位置和钟差的非线性关系。说白了，真实系统几乎都是非线性的。

那怎么办？

一个朴素的想法：既然非线性不好处理，那我们就把它近似成线性的。这就是EKF的核心思想——在估计点附近做泰勒展开，只保留一阶项。

4.2 非线性系统线性化

假设我们有这样一个非线性系统：

状态方程：x_k = f(x_{k-1}, u_{k-1}) + w_{k-1}
观测方程：z_k = h(x_k) + v_k

其中f和h都是非线性函数。w和v是噪声，假设是高斯白噪声。

线性化的过程，就是在当前最优估计点附近，用切线代替原函数。具体来说：

• 对状态方程，在x̂_{k-1}处展开
• 对观测方程，在x̂_k^-处展开

我个人的习惯是，先写出非线性函数，再求偏导。千万别跳步，跳步容易出错。我在项目里吃过这个亏，少算了一个偏导项，结果滤波器发散了一整天。

4.3 雅可比矩阵计算

雅可比矩阵，说白了就是多元函数的一阶偏导数矩阵。在EKF里，我们需要两个雅可比矩阵：

矩阵	含义	维度
F_k	状态转移函数的雅可比	n×n
H_k	观测函数的雅可比	m×n

举个例子，假设状态向量是[x, y, θ]，θ是航向角。状态转移函数是：

x_k = x_{k-1} + v*cos(θ_{k-1})*dt
y_k = y_{k-1} + v*sin(θ_{k-1})*dt
θ_k = θ_{k-1} + ω*dt

那么F_k就是：

F_k = [1, 0, -v*sin(θ)*dt;
       0, 1,  v*cos(θ)*dt;
       0, 0,  1]

注意看第三列，那是θ的偏导。我曾经在写代码时，把sin和cos写反了，结果滤波器估计的轨迹一直在画圈圈。调试了两天才发现，嗯，教训深刻。

4.4 EKF在组合导航中的应用

组合导航里，EKF最常见的应用就是惯导/GPS松组合。我给大家画个流程图，看看数据是怎么流转的。

流程其实不复杂。惯导做时间更新，预测下一时刻的状态。GPS来了，做量测更新，修正预测值。关键就在雅可比矩阵这一步，它决定了线性化近似的好坏。

EKF的完整步骤，我总结成五步：

1. 状态预测：x̂_k^- = f(x̂_{k-1}, u_{k-1})
2. 协方差预测：P_k^- = F_k * P_{k-1} * F_k^T + Q
3. 卡尔曼增益：K_k = P_k^- * H_k^T * (H_k * P_k^- * H_k^T + R)^{-1}
4. 状态更新：x̂_k = x̂_k^- + K_k * (z_k - h(x̂_k^-))
5. 协方差更新：P_k = (I - K_k * H_k) * P_k^-

注意第三步，矩阵求逆。如果H_k的维度很大，求逆会非常耗时。我在做多传感器融合时，观测维度到了20多，求逆一次要好几毫秒。后来改用QR分解，速度快了不少。

4.5 实际项目中的注意事项

最后，我分享几个实战经验：

• 雅可比矩阵要实时更新：别偷懒用常值雅可比，除非你确定系统线性度很好。
• 注意数值稳定性：协方差矩阵要保持对称正定，必要时加一个小的单位矩阵。
• 量测噪声要调好：R矩阵太大，滤波器反应迟钝；太小，容易被噪声带偏。
• 记得做异常检测：GPS偶尔会跳变，用卡方检验检测野值，别让坏数据污染滤波器。

我记得有一次在野外测试，GPS信号被高楼遮挡，位置跳了上百米。幸好提前加了抗差模块，EKF自动降低了GPS的权重，才没让整个系统崩掉。

好了，关于EKF的内容就讲到这里。说白了，它就是一套「打不过就加入」的思路——非线性不好处理，那就近似成线性。虽然不完美，但在工程实践中足够好用。

---