1. 课程导入
在真实工程系统中，状态估计几乎无处不在：无人机要估计姿态与位置，机器人要融合轮速与激光雷达，电池管理系统要推断荷电状态，组合导航要融合惯性测量单元与卫星定位。理想情况下，如果系统是线性的、噪声是高斯的，标准卡尔曼滤波就能给出最优估计。但工程对象的运动学、动力学和观测模型往往是非线性的，例如姿态四元数更新、雷达斜距观测、电池开路电压曲线，这时就需要扩展卡尔曼滤波（Extended Kalman Filter, EKF）。
EKF 的工程价值在于：它用一阶泰勒展开把非线性系统局部线性化，从而复用卡尔曼滤波的最优估计框架。它不追求全局最优，而是在当前估计点附近构造一个可计算的线性近似，使滤波器能够在嵌入式算力受限的条件下实时运行。这也是它至今仍广泛用于飞控、导航、目标跟踪和工业控制的原因。
一、从线性到非线性：EKF 的核心原理
标准卡尔曼滤波假设状态转移和观测都是线性映射：
x_k = F x_{k-1} + B u_k + w_k
z_k = H x_k + v_k其中 w_k 和 v_k 是零均值高斯噪声。EKF 将上述模型推广为非线性函数：
x_k = f(x_{k-1}, u_k) + w_k
z_k = h(x_k) + v_k由于 f 和 h 非线性，高斯分布经过非线性变换后不再保持高斯。EKF 的做法是在当前估计值处求雅可比矩阵，用一阶线性化近似传播均值和协方差：
F_k = ∂f/∂x |_{x=x_{k-1}}
H_k = ∂h/∂x |_{x=x_k^-}
P_k^- = F_k P_{k-1} F_k^T + Q
K_k = P_k^- H_k^T (H_k P_k^- H_k^T + R)^{-1}
x_k = x_k^- + K_k (z_k - h(x_k^-))
P_k = (I - K_k H_k) P_k^-这组公式是 EKF 的工程骨架。理解它的关键不是背公式，而是理解每一步的物理含义：F_k 描述状态不确定性如何随时间传播，H_k 描述观测对状态的敏感度，K_k 决定“信预测还是信观测”，Q 和 R 则决定滤波器的动态响应与平滑程度。
二、方法流程：EKF 一个完整周期的工程步骤
把 EKF 落到代码和工程流程中，一个周期通常包含以下步骤。注意，工程实现中最容易出错的不是公式本身，而是坐标系、时间戳和雅可比符号。
- 状态定义：明确要估计哪些量，例如位置、速度、姿态、零偏。状态维数决定矩阵规模。
- 预测：用非线性状态方程
f推进状态，用雅可比F传播协方差，并叠加过程噪声Q。 - 观测预测：用观测方程
h计算预期观测值，并求雅可比H。 - 计算增益：由
P⁻、H、R计算卡尔曼增益K。 - 更新：用新息
z − h(x⁻)修正状态，并更新协方差。 - 一致性检查：观察新息是否为零均值白噪声，协方差是否与实际误差匹配。
三、典型工程场景：EKF 在做什么
以四旋翼姿态估计为例，状态可定义为姿态四元数和陀螺零偏，观测为加速度计和磁力计。陀螺积分提供高频预测，加速度计和磁力计提供低频修正。EKF 在这里的作用是：在振动、磁干扰和动态加速度并存的情况下，给出连续、平滑且可用的姿态估计。
再以组合导航为例，状态包含位置、速度、姿态和惯性器件零偏，观测为卫星定位结果。EKF 通过 H 矩阵把卫星位置观测映射到状态空间，同时利用 Q 描述惯性器件的随机游走。工程上常见的现象是：Q 太小会导致滤波器过度信任惯性预测，卫星修正被削弱；Q 太大会导致估计噪声明显增大。
| 场景 | 状态量 | 观测量 | 主要难点 |
|---|---|---|---|
| 四旋翼姿态 | 四元数、陀螺零偏 | 加速度计、磁力计 | 动态加速度与磁干扰 |
| 组合导航 | 位置、速度、姿态、零偏 | GNSS 位置/速度 | 坐标系与时间同步 |
| 电池 SOC | 荷电状态、极化电压 | 电压、电流、温度 | 非线性 OCV 曲线 |
| 目标跟踪 | 位置、速度、加速度 | 雷达斜距、方位角 | 观测非线性与角度跳变 |
四、常见误区与复盘
初学者最容易把 EKF 当成“万能滤波”。实际上，EKF 的线性化只在估计点附近有效。如果初始误差过大、非线性过强，或者雅可比推导错误，滤波器可能发散。以下误区值得提前警惕：
- 误区一：忽略雅可比验证。解析雅可比必须用数值差分交叉验证，否则符号错误很难通过观察发现。
- 误区二：Q 和 R 凭感觉设置。它们应与传感器噪声和过程扰动有物理对应关系，最好通过 Allan 方差或实验统计获得初值。
- 误区三：不做一致性检验。新息协方差与理论协方差应大致匹配，否则滤波器可能“看起来收敛，实际不可信”。
- 误区四：忽略数值稳定性。协方差更新建议使用 Joseph 形式或平方根滤波，避免矩阵失去正定性。
本课程后续将沿着“建模—线性化—实现—调参—验证—排障”的主线展开，每一章都围绕可运行的工程案例推进。学习时建议同时准备一份仿真代码和一组真实数据，把每个公式都落到可观测的曲线和指标上。只有把数学、代码和物理对象对应起来，EKF 才会从一组矩阵公式变成真正可用的工程工具。