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