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

本文是 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 的矩阵。

假设我们已知当前的角速度 $ \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} $$

在误差状态模型中，我们需要计算误差状态相对于角速度和旋转的雅可比矩阵 $ 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 $$

当新的测量值（例如视觉 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++ 落地代码，看看这些公式是如何变成实际运行的程序的！