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 的任督二脉。

源码解析 on Galileo Y

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`