EKF算法概述 on Galileo Y

EKF 算法详解：(一) 简介与基础

Thu, 12 Mar 2026 00:00:00 +0800

本文是 EKF 算法详解系列的第一篇。

下一篇：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)

状态预测：根据非线性运动方程推算先验状态。 $$ \hat{x}_k^- = f(\hat{x}_{k-1}, u_{k-1}) $$
协方差预测：通过状态转移的雅可比矩阵 $ F $，更新预测误差协方差矩阵。 $$ P_k^- = F P_{k-1} F^T + Q $$

更新阶段 (Update)

计算卡尔曼增益：利用观测模型的雅可比矩阵 $ H $ 计算增益 $ K $。 $$ K_k = P_k^- H^T (H P_k^- H^T + R)^{-1} $$
状态更新：根据传感器实际测量值 $ z_k $ 与预测值的残差，修正状态，得到后验最优估计。 $$ \hat{x}_k = \hat{x}_k^- + K_k (z_k - h(\hat{x}_k^-)) $$
协方差更新：降低不确定度，更新协方差矩阵供下一轮使用。 $$ P_k = (I - K_k H) P_k^- $$

(注：这里的 $ Q $ 是过程噪声协方差，$ R $ 是测量噪声协方差，它们通常在实际工程中作为“超参数”根据经验进行调参。)

4. 结语与预告

EKF 虽然在数学表达上看起来有些劝退，但只要理解了它**“线性化”与“加权融合”**的核心思想，就能在工程中游刃有余。

在许多实际开源项目（如 RoboMaster 的视觉算法）中，EKF 常被用于追踪装甲板的运动或云台的姿态。在处理三维空间的旋转时，我们为了避免“万向节死锁”和保持约束，通常会引入四元数与误差状态 (Error-State)。

在下一篇文章中，我们将直接进入硬核部分：结合机器人的视觉姿态解算场景，详细推导基于四元数和误差状态的 EKF 数学模型！

EKF 算法详解：(二) 姿态解算与数学推导

Thu, 12 Mar 2026 00:00:00 +0800

本文是 EKF 算法详解系列的第二篇。

上一篇：EKF 算法详解：(一) 简介与基础 | 下一篇：EKF 算法详解：(三) 源码解析实现

在实际的机器人（如 RoboMaster）视觉和云台控制中，扩展卡尔曼滤波（EKF）经常被用于姿态解算（Attitude Estimation）。在本篇文章中，我们将结合实际项目中的四元数（Quaternion）状态，深入推导 EKF 的核心数学模型。

1. 为什么使用四元数与误差状态？

在三维空间中表示旋转，欧拉角容易遇到“万向节死锁”问题，旋转矩阵则需要维护 9 个元素且保持正交约束。四元数 (Quaternion) 只需要 4 个元素，且计算效率高，是姿态表示的最佳选择。

然而，传统的 EKF 难以直接应用于四元数，因为四元数必须满足单位长度约束（$ ||q|| = 1 $）。如果直接对四元数进行线性加减（如 $ q = q + \Delta q $），会破坏其模长。

因此，在实际工程中，我们通常采用 误差状态卡尔曼滤波（Error-State Kalman Filter, ESKF） 或称乘性 EKF（MEKF）：

名义状态 (Nominal State)：用四元数 $ q $ 表示，通过乘法进行更新：$ q_{new} = q \otimes \Delta q $
误差状态 (Error State)：用一个三维向量（误差角 $ \delta \theta $）表示，其协方差矩阵 $ P $ 为 3x3 的矩阵。

2. 状态预测方程 (Prediction)

假设我们已知当前的角速度 $ \omega $ 和角加速度 $ \alpha $，在时间间隔 $ \Delta t $ 内，角度的变化量 $ \Delta \theta $ 可以近似表示为：

$$ \Delta \theta \approx (\omega + \alpha \cdot \Delta t) \Delta t $$

根据角度变化量 $ \Delta \theta $，我们可以构造出一个微小旋转的四元数更新量 $ q_{update} $。设 $ \theta = ||\Delta \theta|| $ 为旋转角，旋转轴 $ u = \frac{\Delta \theta}{\theta} $：

如果旋转角极小（如 $ \theta < 10^{-6} $），根据泰勒展开，四元数更新量可以近似为： $$ q_{update} \approx \begin{bmatrix} 1 \\\\ 0.5 \Delta \theta_x \\\\ 0.5 \Delta \theta_y \\\\ 0.5 \Delta \theta_z \end{bmatrix} $$
如果角度较大，则严格按照四元数轴角公式： $$ q_{update} = \begin{bmatrix} \cos(\frac{\theta}{2}) \\\\ u \sin(\frac{\theta}{2}) \end{bmatrix} $$

最终预测的四元数状态为：

$$ \hat{q}_{k} = q_{k-1} \otimes q_{update} $$

3. 雅可比矩阵的计算 (Jacobian)

在误差状态模型中，我们需要计算误差状态相对于角速度和旋转的雅可比矩阵 $ J $（部分实现中作为观测矩阵或状态转移矩阵的近似）。

定义角速度 $ \omega $ 的反对称矩阵（Skew-symmetric matrix）为 $ [\omega]_\times $：

$$ [\omega]_\times = \begin{bmatrix} 0 & -\omega_z & \omega_y \\\\ \omega_z & 0 & -\omega_x \\\\ -\omega_y & \omega_x & 0 \end{bmatrix} $$

一阶近似下，误差状态转移的雅可比矩阵可以表示为：

$$ J = I_{3\times3} + \frac{1}{2} \Delta t [\omega]_\times $$

4. 状态更新方程 (Update)

当新的测量值（例如视觉 PnP 解算得到的四元数测量值 $ q_{meas} $）到来时，我们需要计算预测值 $ \hat{q}_k $ 与测量值的残差（Residual）：

$$ q_{res} = q_{meas} \otimes \hat{q}_k^{-1} $$

我们提取这个残差四元数的虚部向量部分 $ z_{res} = [q_{res}.x, q_{res}.y, q_{res}.z]^T $ 作为测量残差向量。

接着，计算卡尔曼增益 $ K $。假设我们提取的雅可比矩阵为 $ J $，系统误差协方差为 $ P $，测量噪声协方差为 $ R $：

$$ K = P J^T (J P J^T + R)^{-1} $$

随后将增益乘上残差，得到误差角度修正量：

$$ K_z = K \cdot z_{res} $$

最后，利用这个三维修正量构造误差四元数，乘到预测状态上，并更新协方差矩阵 $ P $：

$$ q_{new} = \hat{q}_k \otimes \begin{bmatrix} 1 \\\\ K_z.x \\\\ K_z.y \\\\ K_z.z \end{bmatrix} $$

$$ P_{new} = (I_{3\times3} - K J) P $$

以上就是一套完整用于姿态跟踪的轻量级 EKF/ESKF 数学推导模型。在下一篇文章中，我们将直接拆解 C++ 落地代码，看看这些公式是如何变成实际运行的程序的！

EKF 算法详解：(三) 源码解析实现

Thu, 12 Mar 2026 00:00:00 +0800

本文是 EKF 算法详解系列的第三篇。

上一篇：EKF 算法详解：(二) 姿态解算与数学推导

在上一篇文章中，我们详细推导了基于四元数和误差状态的 EKF 数学模型。理论推导无论多么完美，最终都需要落地成代码。本文我们将结合一个实际项目的源码（基于 C++ 和 Eigen 库），带你一行行看懂 EKF 的实现细节。

1. 类的定义与初始化 (`ekf.hpp`)

在 C++ 中，我们通常会定义一个 ekf 类来维护卡尔曼滤波器的状态和协方差矩阵。这里我们使用开源且极其强大的矩阵运算库 Eigen。

#ifndef TOOLS__EKF_HPP
#define TOOLS__EKF_HPP
#include <Eigen/Geometry>
#include <eigen3/Eigen/src/Geometry/Quaternion.h>
#include <iostream>

namespace tools
{
class ekf
{
public: 
    ekf();
    ~ekf();
    // 预测步骤：根据角速度和角加速度预测下一个四元数状态
    Eigen::Quaterniond predict(const Eigen::Quaterniond &q, const Eigen::Vector3d &omega, const Eigen::Vector3d &alpha, float dt);
  
    // 计算雅可比矩阵
    Eigen::Matrix3d calculate_jacobian(const Eigen::Quaterniond &q, const Eigen::Vector3d &omega, float dt);

    // 核心更新步骤
    void ekf_update(Eigen::Quaterniond &q, const Eigen::Vector3d &omega, Eigen::Vector3d &alpha, float dt);
 
private:
    // 测量噪声协方差 R 和 状态误差协方差 P
    // 初始化为单位矩阵乘以一个相对较小的置信系数 0.1
    Eigen::Matrix3d R = Eigen::Matrix3d::Identity() * 0.1;
    Eigen::Matrix3d P = Eigen::Matrix3d::Identity() * 0.1;  
};
}
#endif

在这里，值得注意的是协方差矩阵 P 和 R 都是 $3 \times 3$ 的矩阵。这就是我们在上一篇中提到的：状态虽为四元数（4维），但为了满足归一化约束，我们对**误差角（3维）**进行协方差维护。

2. 状态预测函数：`predict`

预测函数的任务是通过当前的角速度 $\omega$ 和角加速度 $\alpha$ 积分，求出这 $\Delta t$ 时间内的微小旋转，然后更新四元数。

Eigen::Quaterniond ekf::predict(const Eigen::Quaterniond &q, const Eigen::Vector3d &omega, const Eigen::Vector3d &alpha, float dt)
{
    // 计算在这 dt 时间内的角度变化量 (delta_theta)
    Eigen::Vector3d delta_theta = (omega + alpha * dt) * dt;
    float theta = delta_theta.norm(); // 求出旋转角的模长
    
    Eigen::Quaterniond q_update;
    
    // 为了防止浮点数精度问题以及计算效率，当角度极小时采用一阶泰勒近似
    if(theta < 1e-6)
    {
        q_update = Eigen::Quaterniond(1, 0.5 * delta_theta.x(), 0.5 * delta_theta.y(), 0.5 * delta_theta.z());
    }
    else 
    {
        // 角度较大时，严格按照轴角转换为四元数
        q_update = Eigen::Quaterniond(Eigen::AngleAxisd(theta, delta_theta.normalized()));
    }
    
    // 四元数乘法完成姿态的预测叠加
    return q * q_update;
}

3. 雅可比矩阵：`calculate_jacobian`

这里构建了反对称矩阵（Skew-symmetric matrix），用于计算雅可比矩阵 $J$：

Eigen::Matrix3d ekf::calculate_jacobian(const Eigen::Quaterniond &q, const Eigen::Vector3d &omega, float dt)
{
    Eigen::Matrix3d j;
    Eigen::Matrix3d omega_skew;

    // 构造角速度的反对称矩阵
    omega_skew << 0,         -omega.z(),  omega.y(),
                  omega.z(),  0,         -omega.x(),
                 -omega.y(),  omega.x(),  0;

    // 根据一阶近似，计算状态转移/观测的雅可比矩阵
    j = Eigen::Matrix3d::Identity() + 0.5 * dt * omega_skew;
    return j;
}

4. EKF 核心更新逻辑：`ekf_update`

这个函数是整个滤波器的灵魂所在，它结合了前两个函数，完成“预测-计算增益-修正-更新协方差”的闭环。

void ekf::ekf_update(Eigen::Quaterniond &q, const Eigen::Vector3d &omega, Eigen::Vector3d &alpha, float dt)
{
    // 1. 预测步：得到先验状态 q_u
    Eigen::Quaterniond q_u = predict(q, omega, alpha, dt);
    
    // 2. 计算雅可比矩阵 J
    Eigen::Matrix3d J = calculate_jacobian(q, omega, dt);
    
    // 3. 计算残差：用实际测量的状态 q 乘以预测状态的逆 q_u.inverse()
    // 注意：在这里的实现中，传入的 q 被视为带有测量信息的最新状态
    Eigen::Quaterniond q_r = q * q_u.inverse();
    // 提取残差四元数的虚部（向量部分）作为测量残差向量
    Eigen::Vector3d z_r(q_r.x(), q_r.y(), q_r.z());

    // 4. 计算卡尔曼增益 K
    // 公式: K = P * H^T * (H * P * H^T + R)^-1
    // (代码将雅可比矩阵 J 作为 H 矩阵直接代入观测模型中计算)
    Eigen::Matrix3d K = P * J.transpose() * (J * P * J.transpose() + R).inverse();
    
    // 5. 计算误差修正量
    Eigen::Vector3d Kz = K * z_r;
    
    // 6. 后验状态更新：将算出的误差转换为四元数，乘上之前的预测值
    q = q_u * Eigen::Quaterniond(1, Kz[0], Kz[1], Kz[2]);
    
    // 7. 更新协方差矩阵 P
    P = (Eigen::Matrix3d::Identity() - K * J) * P;
}

总结

这段代码是一个非常轻量且实用的工程化 EKF 实现，它舍弃了标准 EKF 中过于繁琐的矩阵运算（例如舍弃了复杂的高维状态转移阵，精简了观测矩阵提取的步骤），直接针对“四元数位姿跟踪”这一特定场景进行了极致优化。通过协方差降维和一阶近似泰勒展开，使得这套算法可以在算力有限的嵌入式设备（如各类机器人的下位机/视觉板）上高速运行。

至此，我们的 EKF 算法详解系列就全部结束了！希望这三篇文章能帮你从概念到数学，再到代码落地，彻底打通 EKF 的任督二脉。

EKF算法概述 on Galileo Y

EKF 算法详解：(一) 简介与基础

1. 什么是卡尔曼滤波 (Kalman Filter)?

标准 KF 的局限性

2. 为什么需要扩展卡尔曼滤波 (EKF)?

EKF 的优势：

3. EKF 算法的通用五步公式

预测阶段 (Predict)

更新阶段 (Update)

4. 结语与预告

EKF 算法详解：(二) 姿态解算与数学推导

1. 为什么使用四元数与误差状态？

2. 状态预测方程 (Prediction)

3. 雅可比矩阵的计算 (Jacobian)

4. 状态更新方程 (Update)

EKF 算法详解：(三) 源码解析实现

1. 类的定义与初始化 (ekf.hpp)

2. 状态预测函数：predict

3. 雅可比矩阵：calculate_jacobian

4. EKF 核心更新逻辑：ekf_update

总结

1. 类的定义与初始化 (`ekf.hpp`)

2. 状态预测函数：`predict`

3. 雅可比矩阵：`calculate_jacobian`

4. EKF 核心更新逻辑：`ekf_update`