05 · 深入｜ESKF：姿态为什么用小误差来估计

四元数有四个数，但只有三个自由度

完整状态可以写成位置 p、速度 v、姿态四元数 q、加速度偏置 b_a、陀螺偏置 b_g。四元数受单位范数约束，因此姿态误差更适合用三维小旋转 δθ 表示：名义状态负责大运动，误差状态负责局部纠正。这里采用右扰动 R_true=R_hat Exp([δθ]×)。

$$\text{δx =}{\left[\begin{array}{ccccc}\text{δp}& \text{δv}& \text{δθ}& {\text{δb}}_{\text{a}}& {\text{δb}}_{\text{g}}\end{array}\right]}^{\text{T}}\text{∈}{\text{ℝ}}^{\text{15}}$$

IMU 加速度计测的是比力而非包含重力的导航系加速度。静止水平、FRD 坐标下，比力约为 [0,0,−g]，经过旋转再加重力后，导航系加速度才应为零。

$$\text{ṗ = v， v̇ = R(a\_m−b\_a)+gⁿ， Ṙ = R[ω\_m−b\_g]×}$$

$$\text{δv̇ = −R[f̂]×δθ − Rδb\_a − Rn\_a}$$

这个误差方程最值得理解的是第一项：姿态估错会把重力或比力投到错误方向，因此一点倾角误差就可能造成显著水平漂移。δθ̇=−[ω̂]×δθ−δb_g−n_g 则说明陀螺偏置怎样逐渐变成姿态误差。

DVL 和深度如何接进来

设 C 把机体系旋转到 DVL 系，r 为 DVL 杆臂，则底跟踪测量模型为：

$$\text{z\_DVL = C(Rᵀvⁿ + ω̂×r) + n\_DVL}$$

无杆臂时速度雅可比为 CRᵀ，右姿态误差雅可比为 C[Rᵀv]×。有杆臂时，角速率与陀螺偏置也会影响测量。压力深度模型为 z_h=e₃ᵀ(p+Rr_h)+b_h+n_h；压力计偏置与绝对深度需要参考基准才能分离。压力深度与离底高度是不同量。

更新后还要重置误差坐标

得到 δx 后，位置、速度和偏置相加，姿态使用 R←R Exp([δθ]×)。随后误差状态归零，但协方差还要变换到新的局部坐标。右扰动的一阶姿态重置块是 J_θ≈I−½[δθ]×，并做 P←JPJᵀ。遗漏这一步，相当于把旧切平面的不确定度直接搬到新姿态上。

算例与实验

仅 1° 倾角误差就能泄漏约 g sin1°≈0.171 m/s² 的水平重力分量，持续 3 s 的位置误差量级约 0.77 m。这说明 DVL 短时缺测时，姿态与偏置质量往往比简单“保持上次速度”更关键。实现时先对静止、匀速、原地转动三类轨迹检查，再用数值差分比较测量模型与雅可比；一种扰动定义从头用到底，比混用几个来源的公式更重要。

阅读：Solà《Quaternion kinematics for the error-state Kalman filter》，重点看误差定义、注入与重置：https://arxiv.org/abs/1711.02508 。