4、扩展卡尔曼滤波：非线性系统线性化、雅可比矩阵计算、EKF在定位中的应用
说到卡尔曼滤波，很多做定位的朋友第一反应就是「线性、高斯」。但现实世界哪有那么完美？你想想看，我们用的传感器——IMU、里程计、GPS——哪个不是非线性的？我刚开始做机器人定位那会儿，天真地以为套个标准KF就能搞定，结果跑起来没几分钟就发散得一塌糊涂。后来才明白，真实系统几乎都是非线性的，这时候就得请出扩展卡尔曼滤波（EKF）了。
4.1 为什么需要EKF？非线性系统的困境
标准卡尔曼滤波有个硬前提：系统必须是线性的。说白了，就是状态转移和观测方程都必须是线性函数。但实际中呢？
- 运动模型非线性：比如轮式机器人的运动，角度变化带来的位移是三角函数关系，不是简单的加减乘除
- 观测模型非线性：比如激光雷达测距，距离和角度到笛卡尔坐标的转换，也是非线性的
- IMU积分：加速度和角速度积分得到位置和姿态，这过程本身就是非线性
我遇到过最典型的例子：一个AGV小车，用标准KF处理里程计数据，转弯稍微急一点，估计的位置就直接飞到墙外面去了。为什么会这样？因为标准KF假设状态转移是线性的，但实际转弯时的运动方程包含sin和cos，线性近似根本扛不住。
核心思想：EKF的核心就是「打不过就加入」——既然系统是非线性的，那就在每个时间点对非线性函数做线性化近似，然后套用标准KF的框架。
4.2 非线性系统线性化：泰勒展开的艺术
线性化的数学工具很简单——泰勒展开。我习惯只保留一阶项，忽略高阶项。为什么？因为计算量可控，而且对于大多数定位场景，一阶近似已经够用了。
假设我们的非线性系统是这样的：
状态方程：x_k = f(x_{k-1}, u_k) + w_k
观测方程：z_k = h(x_k) + v_k其中f和h都是非线性函数。线性化的做法是：
- 在上一时刻的最优估计点x̂_{k-1}附近，对f做泰勒展开
- 在当前时刻的预测点x̂_k^-附近，对h做泰勒展开
- 只保留一阶项，忽略二阶及以上
嗯，这里要注意：线性化的精度取决于两个因素——非线性程度和采样频率。如果系统非线性特别强，或者采样频率太低，一阶近似可能不够。我在做无人机定位时遇到过这种情况，后来不得不改用UKF（无迹卡尔曼滤波）。
4.3 雅可比矩阵计算：EKF的「发动机」
雅可比矩阵就是泰勒展开中的一阶偏导数矩阵。说白了，它告诉我们：当输入变化一点点时，输出会变化多少。在EKF里，我们需要计算两个雅可比矩阵：
| 矩阵 | 含义 | 维度 |
|---|---|---|
| F_k | 状态转移函数f对状态x的偏导 | n×n |
| H_k | 观测函数h对状态x的偏导 | m×n |
举个例子，假设一个简单的2D机器人，状态为[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]]我刚开始手算雅可比时经常出错，后来养成了一个习惯：先用符号推导一遍，再用数值方法验证。比如用有限差分法算一遍，跟解析解对比，差太多就说明算错了。
个人技巧：如果你用的是Python，可以用SymPy做符号求导，自动生成雅可比矩阵的代码。我在项目中经常这么干，省时省力还不会出错。
4.4 EKF在定位中的完整流程
EKF的流程跟标准KF很像，只是把线性矩阵换成了雅可比矩阵。我习惯把它分成两步：预测和更新。
4.4.1 预测步骤
// 状态预测（用非线性函数f）
x̂_k^- = f(x̂_{k-1}, u_k)
// 协方差预测（用雅可比矩阵F_k）
P_k^- = F_k * P_{k-1} * F_k^T + Q_k这里有个坑：状态预测用的是原始的非线性函数f，而不是线性化后的版本。只有协方差传播才用雅可比矩阵。我曾经犯过这个错误，把状态预测也用了线性近似，结果精度差了很多。
4.4.2 更新步骤
// 计算卡尔曼增益
K_k = P_k^- * H_k^T * (H_k * P_k^- * H_k^T + R_k)^{-1}
// 状态更新
x̂_k = x̂_k^- + K_k * (z_k - h(x̂_k^-))
// 协方差更新
P_k = (I - K_k * H_k) * P_k^-注意观测残差的计算：z_k - h(x̂_k^-)。这里h也是非线性函数，直接用原始函数计算预测观测值，而不是用线性近似。
4.5 实战案例：轮式机器人定位
我在做一个仓储机器人项目时，用EKF融合了里程计和激光雷达数据。里程计提供高频但会漂移的位姿估计，激光雷达提供低频但绝对的位置修正。
状态向量是7维：[x, y, θ, v, ω, bias_v, bias_ω]
运动模型用的是自行车模型：
x_k = x_{k-1} + (v + bias_v)*cos(θ_{k-1})*dt
y_k = y_{k-1} + (v + bias_v)*sin(θ_{k-1})*dt
θ_k = θ_{k-1} + (ω + bias_ω)*dt观测模型是激光雷达检测到已知路标点：
range = sqrt((x - x_landmark)^2 + (y - y_landmark)^2)
bearing = atan2(y_landmark - y, x_landmark - x) - θ这个观测模型是非线性的，所以需要计算H_k。我算出来的雅可比是：
H_k = [[(x - x_l)/r, (y - y_l)/r, 0, 0, 0, 0, 0],
[-(y - y_l)/r^2, (x - x_l)/r^2, -1, 0, 0, 0, 0]]其中r = sqrt((x - x_l)^2 + (y - y_l)^2)
避坑指南：我曾经在计算bearing的雅可比时，忘了对θ求偏导，结果角度估计一直收敛不了。后来花了整整一天调试才发现这个错误。所以算雅可比时，一定要检查每个状态变量对观测的影响。
4.6 EKF的局限性
EKF虽然好用，但不是万能的。我总结了几点：
- 强非线性场景表现不佳：比如大角度转弯、剧烈加速，一阶近似误差会累积
- 雅可比计算复杂：对于高维状态或复杂模型，手算容易出错
- 对初值敏感：如果初始估计偏差太大，线性化点选得不好，滤波器可能直接发散
遇到这些情况，我一般会考虑UKF或粒子滤波。但话说回来，对于大多数定位场景，EKF仍然是性价比最高的选择——实现简单、计算量小、效果够用。
总结一下：EKF的核心就三件事——找对非线性函数、算对雅可比矩阵、套对标准KF框架。只要这三步不出错，EKF在定位中的表现通常不会让你失望。
可可精品资料库