卡尔曼滤波那篇 结尾提到：姿态估计要用误差状态卡尔曼滤波（ESKF）。这篇把“要用”展开成“为什么以及怎么用”。IMU 姿态估计是每个机器人——无人机、足式、机械臂基座、手持设备——都绕不开的模块，它也是理解 VIO、组合导航这些更大系统的最小完整样本。
一颗 MEMS IMU 输出两组三轴测量，各自的误差模型必须先写清楚，后面滤波器的 Q \mathbf Q Q
ω m = ω + b g + n g , b ˙ g = n b g a m = R ⊤ ( a W − g ) + b a + n a , b ˙ a = n b a \begin{aligned}
\boldsymbol\omega_m &= \boldsymbol\omega + \mathbf b_g + \mathbf n_g,
\qquad \dot{\mathbf b}g = \mathbf n {bg} \
\mathbf a_m &= \mathbf R^\top(\mathbf a^W - \mathbf g) + \mathbf b_a + \mathbf n_a,
\qquad \dot{\mathbf b}a = \mathbf n {ba}
\end{aligned} ω m a m = ω + b g + n g , b ˙ g = n b g = R ⊤ ( a W − g ) + b a + n a , b ˙ a = n ba
陀螺仪 测角速度，误差 = 白噪声 n g \mathbf n_g n g 缓慢游走的偏置 b g \mathbf b_g b g 加速度计 测的是比力 （specific force）——重力与运动加速度之和在机体系的投影，同样带白噪声和偏置。
两类传感器的角色由误差的频谱决定：陀螺仪积分短期极准 （毫秒到秒级），但偏置积分成角度后无界漂移 ；加速度计静止时给出重力方向 （绝对的俯仰/横滚参考，长期无漂移），但瞬时噪声大，且机体一加速，“重力方向”就被线加速度污染。姿态融合的本质是按频段分工 ：高频信陀螺，低频信重力。
仿真对比（真实生成）：纯陀螺积分漂移无界；加速度计瞬时噪声大且在线加速度段（黄色区间）给出完全错误的“倾角”；互补滤波和 KF 都实现了频段分工，但 KF 在线估计偏置，扰动后恢复更快、长期更稳
参数从哪来？数据手册与 Allan 方差 标定。把 IMU 静置数小时，对不同平均时长 τ \tau τ − 1 / 2 -1/2 − 1/2 角度随机游走 （白噪声密度，决定 n g \mathbf n_g n g 偏置不稳定性 （决定 n b g \mathbf n_{bg} n b g Q \mathbf Q Q
姿态用单位四元数 q \mathbf q q
约束 ：∥ q ∥ = 1 \|\mathbf q\| = 1 ∥ q ∥ = 1 x ^ + K y \hat{\mathbf x} + \mathbf K\mathbf y x ^ + Ky 过参数化 ：4 个数表示 3 个自由度，4 × 4 4\times4 4 × 4 加法不是那么回事 ：旋转的复合是乘法（群运算），q 1 + q 2 \mathbf q_1 + \mathbf q_2 q 1 + q 2 S O ( 3 ) SO(3) S O ( 3 )
ESKF 的解法可以概括成一句话：让非线性归非线性，让高斯归高斯 。
把真实状态拆成两部分：
q t r u e = q n o m ⊗ δ q ( δ θ ) , b t r u e = b n o m + δ b \mathbf q_{true} = \mathbf q_{nom} \otimes \delta\mathbf q(\delta\boldsymbol\theta),
\qquad
\mathbf b_{true} = \mathbf b_{nom} + \delta\mathbf b q t r u e = q n o m ⊗ δ q ( δ θ ) , b t r u e = b n o m + δ b
名义状态 （q n o m , b n o m \mathbf q_{nom}, \mathbf b_{nom} q n o m , b n o m 误差状态 （δ θ ∈ R 3 , δ b \delta\boldsymbol\theta \in \mathbb R^3, \delta\mathbf b δ θ ∈ R 3 , δ b 三维小角度向量 表示（δ q ≈ [ 1 , 1 2 δ θ ] ⊤ \delta\mathbf q \approx [1, \tfrac12\delta\boldsymbol\theta]^\top δ q ≈ [ 1 , 2 1 δ θ ] ⊤
三个问题同时消解：误差状态是自由向量（无约束）、维数恰好 3（协方差满秩）、而且因为它始终很小，线性化几乎不引入误差 ——这正是 EKF 线性化误差 的对症药：EKF 在大状态上线性化，ESKF 永远在原点附近线性化。
预测 ：名义四元数按陀螺积分（零阶保持下的增量旋转 δ ϕ = ( ω m − b g ) Δ t \delta\boldsymbol\phi = (\boldsymbol\omega_m - \mathbf b_g)\Delta t δ ϕ = ( ω m − b g ) Δ t
P ← F P F ⊤ + Q , F = [ exp ( − [ δ ϕ ] × ) − I Δ t 0 I ] \mathbf P \leftarrow \mathbf F,\mathbf P,\mathbf F^\top + \mathbf Q, \qquad
\mathbf F = \begin{bmatrix}
\exp(-[\delta\boldsymbol\phi]_\times) & -\mathbf I,\Delta t \
\mathbf 0 & \mathbf I
\end{bmatrix} P ← F P F ⊤ + Q , F = [ exp ( − [ δ ϕ ] × ) 0 − I Δ t I ]
F \mathbf F F − I Δ t -\mathbf I\Delta t − I Δ t Q \mathbf Q Q
F \mathbf F F 姿态误差 δ θ \delta\boldsymbol\theta δ θ 。
用旋转矩阵形式最干净。误差定义 R t r u e = R n o m δ R \mathbf R_{true} = \mathbf R_{nom}\,\delta\mathbf R R t r u e = R n o m δ R δ R ≈ I + [ δ θ ] × \delta\mathbf R \approx \mathbf I + [\delta\boldsymbol\theta]_\times δ R ≈ I + [ δ θ ] ×
R ˙ t r u e = R t r u e [ ω t r u e ] × , R ˙ n o m = R n o m [ ω ^ ] × \dot{\mathbf R}{true} = \mathbf R {true} [\boldsymbol\omega_{true}]\times,
\qquad
\dot{\mathbf R} {nom} = \mathbf R_{nom} [\hat{\boldsymbol\omega}]_\times R ˙ t r u e = R t r u e [ ω t r u e ] × , R ˙ n o m = R n o m [ ω ^ ] ×
其中 ω ^ = ω m − b n o m \hat{\boldsymbol\omega} = \boldsymbol\omega_m - \mathbf b_{nom} ω ^ = ω m − b n o m ω t r u e = ω m − b t r u e − n g = ω ^ − δ b − n g \boldsymbol\omega_{true} = \boldsymbol\omega_m - \mathbf b_{true} - \mathbf n_g = \hat{\boldsymbol\omega} - \delta\mathbf b - \mathbf n_g ω t r u e = ω m − b t r u e − n g = ω ^ − δ b − n g 误差的源头就是这两者之差 。
对 R t r u e = R n o m δ R \mathbf R_{true} = \mathbf R_{nom}\delta\mathbf R R t r u e = R n o m δ R R n o m ⊤ \mathbf R_{nom}^\top R n o m ⊤
δ R ˙ = δ R [ ω t r u e ] × − [ ω ^ ] × δ R \delta\dot{\mathbf R} = \delta\mathbf R, [\boldsymbol\omega_{true}]\times - [\hat{\boldsymbol\omega}] \times, \delta\mathbf R δ R ˙ = δ R [ ω t r u e ] × − [ ω ^ ] × δ R
代入一阶近似 δ R ≈ I + [ δ θ ] × \delta\mathbf R \approx \mathbf I + [\delta\boldsymbol\theta]_\times δ R ≈ I + [ δ θ ] × δ R ˙ ≈ [ δ θ ˙ ] × \delta\dot{\mathbf R} \approx [\delta\dot{\boldsymbol\theta}]_\times δ R ˙ ≈ [ δ θ ˙ ] × [ δ θ ] × [ δ b ] × [\delta\boldsymbol\theta]_\times[\delta\mathbf b]_\times [ δ θ ] × [ δ b ] ×
[ δ θ ˙ ] × = [ δ θ ] × [ ω ^ ] × − [ ω ^ ] × [ δ θ ] × ⏟ 对易子 − [ δ b + n g ] × [\delta\dot{\boldsymbol\theta}]\times
= \underbrace{[\delta\boldsymbol\theta] \times [\hat{\boldsymbol\omega}]\times - [\hat{\boldsymbol\omega}] \times [\delta\boldsymbol\theta]\times} {\text{对易子}} ;-; [\delta\mathbf b + \mathbf n_g]_\times [ δ θ ˙ ] × = 对易子 [ δ θ ] × [ ω ^ ] × − [ ω ^ ] × [ δ θ ] × − [ δ b + n g ] ×
对易子用叉乘恒等式 [ a ] × [ b ] × − [ b ] × [ a ] × = [ a × b ] × [\mathbf a]_\times[\mathbf b]_\times - [\mathbf b]_\times[\mathbf a]_\times = [\mathbf a \times \mathbf b]_\times [ a ] × [ b ] × − [ b ] × [ a ] × = [ a × b ] × [ ⋅ ] × [\cdot]_\times [ ⋅ ] ×
δ θ ˙ = − ω ^ × δ θ − δ b − n g δ b ˙ = n b g \boxed{;\delta\dot{\boldsymbol\theta} = -\hat{\boldsymbol\omega} \times \delta\boldsymbol\theta - \delta\mathbf b - \mathbf n_g;}
\qquad
\delta\dot{\mathbf b} = \mathbf n_{bg} δ θ ˙ = − ω ^ × δ θ − δ b − n g δ b ˙ = n b g
三项各有含义：− ω ^ × δ θ -\hat{\boldsymbol\omega}\times\delta\boldsymbol\theta − ω ^ × δ θ 跟着坐标系转 （误差表示在体坐标系里，载体转了、误差的分量表达就变）；− δ b -\delta\mathbf b − δ b n g \mathbf n_g n g
对这组线性微分方程做 Δ t \Delta t Δ t F \mathbf F F δ θ \delta\boldsymbol\theta δ θ exp ( − [ ω ^ Δ t ] × ) = exp ( − [ δ ϕ ] × ) \exp(-[\hat{\boldsymbol\omega}\Delta t]_\times) = \exp(-[\delta\boldsymbol\phi]_\times) exp ( − [ ω ^ Δ t ] × ) = exp ( − [ δ ϕ ] × ) − I Δ t -\mathbf I\Delta t − I Δ t − δ b -\delta\mathbf b − δ b 整个滤波器里唯一的近似就是“丢掉二阶小量”这一步 ，而误差状态每个周期都被复位到零，二阶小量是真的小——这就是 ESKF 线性化质量优于 EKF 的推导层面依据。完整的含速度、位置的 15 维版本见 Solà 的讲义，套路与此完全相同。
更新 ：静止或缓变时，加速度计的测量方向应等于名义姿态预测的重力方向。残差
y = a m / ∥ a m ∥ − R ⊤ ( q n o m ) g / g \mathbf y = \mathbf a_m / |\mathbf a_m| - \mathbf R^\top(\mathbf q_{nom}),\mathbf g / g y = a m /∥ a m ∥ − R ⊤ ( q n o m ) g / g
对误差状态的雅可比 H = [ [ R ⊤ g ] × / g 0 ] \mathbf H = [\,[\mathbf R^\top\mathbf g]_\times / g \;\; \mathbf 0\,] H = [ [ R ⊤ g ] × / g 0 ] δ θ ^ , δ b ^ \delta\hat{\boldsymbol\theta}, \delta\hat{\mathbf b} δ θ ^ , δ b ^ 俯仰和横滚 ——绕重力轴的偏航对重力方向不敏感（H \mathbf H H
注入与复位 ：把估出的误差并回名义状态——四元数乘上 δ q ( δ θ ^ ) \delta\mathbf q(\delta\hat{\boldsymbol\theta}) δ q ( δ θ ^ )
flowchart LR
A[陀螺 200 Hz~1 kHz] --> B[名义状态积分<br/>完整非线性、四元数乘法]
B --> C[误差协方差传播<br/>F、Q 线性传播]
D[加速度计<br/>过χ²门限] --> E[KF 更新误差状态<br/>δθ、δb]
C --> E
E --> F[注入：q ← q ⊗ δq<br/>b ← b + δb]
F --> G[误差清零、协方差复位]
G --> B
加速度计更新的前提“测的是纯重力”在机体加速时不成立（图中黄色区间）。三层防御依次是：
门限 ：∣ ∥ a m ∥ − g ∣ \left|\,\|\mathbf a_m\| - g\,\right| ∣ ∥ a m ∥ − g ∣ g g g 自适应 R \mathbf R R ：不粗暴跳过，而是按偏离程度膨胀测量噪声，让更新平滑降权；把线加速度建进模型 ：无人机可以用动力学模型（推力/阻力）预测比力，足式机器人用触地状态判断哪几段可信——这已经在往组合导航的方向走了。
图里 KF 与互补滤波在扰动段的差异就来自这层处理加上偏置估计：互补滤波以固定比例吞下被污染的加速度计读数，KF 可以拒绝或降权，且扰动期间靠已估准的偏置维持纯积分精度。
互补滤波一行就能写完：
θ ^ k = α ( θ ^ k − 1 + ω m Δ t ) + ( 1 − α ) θ a c c \hat\theta_k = \alpha\left(\hat\theta_{k-1} + \omega_m \Delta t\right) + (1-\alpha),\theta_{acc} θ ^ k = α ( θ ^ k − 1 + ω m Δ t ) + ( 1 − α ) θ a cc
高通给陀螺、低通给加速度计，α = τ / ( τ + Δ t ) \alpha = \tau/(\tau + \Delta t) α = τ / ( τ + Δ t ) S O ( 3 ) SO(3) S O ( 3 ) 在噪声平稳、动态温和的场景，它和 ESKF 的精度差距很小 ——这是必须诚实承认的。
ESKF 多出来的复杂度买到四样东西：
能力
互补滤波
ESKF
偏置在线估计
无（偏置直接漏进姿态）
状态之一，温漂自动跟踪
不确定度输出
无
协方差，供下游（规划、控制）消费
增益
固定分频点，全场景一套
随协方差自动调节：初始化快收敛、稳态低噪声
扩展性
到此为止
加状态就是 VIO / GNSS 组合导航 / 轮式里程计融合
最后一条是本质的：ESKF 的名义/误差双轨结构原封不动地就是 VIO（MSCKF、VINS 的滤波内核）和 GNSS/INS 组合导航的骨架——状态从 6 维扩到位置、速度、外参、特征深度，套路完全一样。学 ESKF 不是为了姿态那 6 个状态，是为了这个可扩展的框架。
初始化 ：静置几秒，加速度计均值定横滚/俯仰，陀螺均值直接作为 b g \mathbf b_g b g 静止检测送温补 ：检测到静止（角速度模长和加速度模长都稳定）时可以加“零角速度更新”（陀螺读数全额记为偏置观测），温漂跟踪立竿见影；振动 ：电机振动频率高于采样奈奎斯特频率时会混叠 进低频段，任何滤波器都救不回来——先机械减振（隔振垫），再抬采样率，最后才是数字滤波；时间戳 ：融合多传感器时对齐时间戳比调 Q / R \mathbf Q/\mathbf R Q / R 验证 ：和上一篇 一样，NIS 一致性监控要常开；姿态真值可用转台或视觉动捕，没有条件时至少做“积分-回零”测试（缓慢转一圈回到原位，看姿态是否归位）。
IMU 融合的物理本质是频段分工 ：陀螺管高频、重力参考管低频；一切方案（互补、Mahony、ESKF）都是这句话的不同实现精度。
四元数不能直接进 KF 的三个原因——单位约束、协方差奇异、加法无意义——被 ESKF 的切空间误差状态 一次性解决。
ESKF = 名义状态非线性积分 + 误差状态线性滤波 + 注入复位；因为误差永远在原点附近，线性化质量天然优于 EKF。
Q \mathbf Q Q 简单场景用互补滤波不丢人；要偏置跟踪、不确定度和向 VIO/组合导航扩展的路，选 ESKF。
J. Solà. Quaternion kinematics for the error-state Kalman filter . arXiv:1711.02508.（ESKF 最完整的推导，本文记号与其一致）
N. Trawny, S. I. Roumeliotis. Indirect Kalman Filter for 3D Attitude Estimation . Tech. Report, 2005.
R. Mahony, T. Hamel, J.-M. Pflimlin. Nonlinear Complementary Filters on the Special Orthogonal Group . IEEE TAC, 2008.
IEEE Std 952: Allan 方差在陀螺标定中的标准方法。
O. J. Woodman. An Introduction to Inertial Navigation . Cambridge Tech. Report, 2007.（IMU 误差模型入门最佳）
仿真对比（真实生成）：纯陀螺积分漂移无界；加速度计瞬时噪声大且在线加速度段（黄色区间）给出完全错误的“倾角”；互补滤波和 KF 都实现了频段分工，但 KF 在线估计偏置，扰动后恢复更快、长期更稳