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++ 落地代码，看看这些公式是如何变成实际运行的程序的！