本文是 EKF 算法详解系列 的第一篇。
在机器人、自动驾驶、无人机等领域中，“状态估计”是一个绕不开的核心问题。无论是云台的姿态解算，还是车辆的轨迹跟踪，我们都希望知道系统在当前时刻的准确状态。而在众多状态估计算法中，扩展卡尔曼滤波（Extended Kalman Filter, 简称 EKF） 无疑是最经典、应用最广泛的算法之一。
本系列文章将带你从零开始，理解 EKF 算法。本文为第一篇，主要介绍 EKF 的基本概念、它与标准卡尔曼滤波的区别，以及它的核心思想。
1. 什么是卡尔曼滤波 (Kalman Filter)?
在介绍 EKF 之前，我们需要先简单回顾一下标准卡尔曼滤波 (KF)。
在现实世界中，任何传感器都有噪声（误差），任何物理模型也都无法做到 100% 完美预测。卡尔曼滤波的本质，就是融合“系统模型的预测”与“传感器的实际测量”，通过数学推导，找出一个在统计意义上“最靠谱”的最优估计值。
卡尔曼滤波主要分为两个不断循环的步骤：
- 预测 (Predict)：根据上一时刻的状态和物理模型（如运动学方程：位移 = 速度 × 时间），推算出当前时刻的“先验状态”。
- 更新 (Update)：读取传感器的测量值，计算测量值与预测值之间的差异（残差），然后通过“卡尔曼增益”来动态决定是更相信模型预测，还是更相信传感器测量，最终得到当前时刻的“后验状态”。
标准 KF 的局限性
标准卡尔曼滤波有一个致命的假设前提：系统必须是线性的。也就是说，状态的转移和观测过程都必须能用线性矩阵相乘来表示。 但在真实世界里，比如角度的旋转涉及正弦/余弦运算（如欧拉角、四元数），或者是复杂的阻力模型，这些都是非线性的。这时候，标准 KF 就会失效。
2. 为什么需要扩展卡尔曼滤波 (EKF)?
既然现实系统大多是非线性的，我们就需要对标准卡尔曼滤波进行“扩展”——这就是 EKF 诞生的原因。
EKF 的核心思想非常简单粗暴但又极其有效：既然系统是非线性的，那就在当前工作点附近把它“局部线性化”。
具体来说，EKF 利用了高等数学中的泰勒展开（Taylor Expansion）。它将非线性的状态转移函数和观测函数在当前的最优估计值处进行一阶泰勒展开，忽略掉高阶项。这样一来，复杂的非线性函数就被近似成了一个线性模型（即求导数，得到雅可比矩阵 Jacobian）。
EKF 的优势：
- 适用性广：完美解决了非线性系统的状态估计问题（例如机器人的视觉位姿追踪）。
- 计算效率高：相比于粒子滤波（PF）或无迹卡尔曼滤波（UKF），EKF 只需要计算一阶雅可比矩阵，计算量较小，非常适合部署在算力受限的嵌入式设备（如各类单片机、STM32 等）上。
3. EKF 算法的通用五步公式
一个标准的 EKF 同样遵循“预测”和“更新”两大阶段，具体的数学公式通常被归纳为以下经典的五步：
预测阶段 (Predict)
- 状态预测：根据非线性运动方程推算先验状态。
- 协方差预测：通过状态转移的雅可比矩阵 ( F )，更新预测误差协方差矩阵。
更新阶段 (Update)
- 计算卡尔曼增益：利用观测模型的雅可比矩阵 ( H ) 计算增益 ( K )。
- 状态更新：根据传感器实际测量值 ( z_k ) 与预测值的残差，修正状态，得到后验最优估计。
- 协方差更新：降低不确定度，更新协方差矩阵供下一轮使用。
(注：这里的 ( Q ) 是过程噪声协方差，( R ) 是测量噪声协方差，它们通常在实际工程中作为“超参数”根据经验进行调参。)
4. 结语与预告
EKF 虽然在数学表达上看起来有些劝退，但只要理解了它**“线性化”与“加权融合”**的核心思想，就能在工程中游刃有余。
在许多实际开源项目（如 RoboMaster 的视觉算法）中，EKF 常被用于追踪装甲板的运动或云台的姿态。在处理三维空间的旋转时，我们为了避免“万向节死锁”和保持约束，通常会引入四元数与误差状态 (Error-State)。
在下一篇文章中，我们将直接进入硬核部分：结合机器人的视觉姿态解算场景，详细推导基于四元数和误差状态的 EKF 数学模型！