PX4状态估计技术EKF2详解（二）：EKF2 误差状态动力学与协方差传播

EKF2 的核心数学问题是误差状态的传播。标称状态做 IMU 积分，误差状态做协方差传播。本文从 ESKF 框架出发，逐步推导姿态、速度、位置、偏置的误差传播方程，构建完整的状态转移矩阵 F \mathbf{F} F 和噪声协方差矩阵 Q \mathbf{Q} Q，最后简要说明实现方式。

1. 状态空间与坐标约定

1.1 原始状态与误差状态

EKF2 使用 24 维误差状态卡尔曼滤波器（ESKF）。状态分为原始状态（storage）和误差状态（tangent）：

原始状态 x \mathbf{x} x（25 维 storage）：

x = [ q T , v T , p T , b g T , b a T , m I T , m B T , v w T , h t e r r ] T \mathbf{x} = \left[ \mathbf{q}^T, \mathbf{v}^T, \mathbf{p}^T, \mathbf{b}_g^T, \mathbf{b}_a^T, \mathbf{m}_I^T, \mathbf{m}_B^T, \mathbf{v}w^T, h{terr} \right]^T x=[qT,vT,pT,bgT,baT,mIT,mBT,vwT,hterr]T

其中 q ∈ S 3 \mathbf{q} \in \mathbb{S}^3 q∈S3 为单位四元数， v , p , b g , b a , m I , m B ∈ R 3 \mathbf{v}, \mathbf{p}, \mathbf{b}_g, \mathbf{b}_a, \mathbf{m}_I, \mathbf{m}_B \in \mathbb{R}^3 v,p,bg,ba,mI,mB∈R3， v w ∈ R 2 \mathbf{v}w \in \mathbb{R}^2 vw∈R2， h t e r r ∈ R h{terr} \in \mathbb{R} hterr∈R。

误差状态 δ x \delta\mathbf{x} δx（24 维 tangent）：

δ x = [ δ θ T , δ v T , δ p T , δ b g T , δ b a T , δ m I T , δ m B T , δ v w T , δ h t e r r ] T \delta\mathbf{x} = \left[ \delta\boldsymbol{\theta}^T, \delta\mathbf{v}^T, \delta\mathbf{p}^T, \delta\mathbf{b}_g^T, \delta\mathbf{b}_a^T, \delta\mathbf{m}_I^T, \delta\mathbf{m}_B^T, \delta\mathbf{v}w^T, \delta h{terr} \right]^T δx=[δθT,δvT,δpT,δbgT,δbaT,δmIT,δmBT,δvwT,δhterr]T

姿态误差用旋转矢量 δ θ ∈ R 3 \delta\boldsymbol{\theta} \in \mathbb{R}^3 δθ∈R3 表示（四元数的切空间），其余状态误差为简单加法。

1.2 标称-误差的复合关系

姿态：

q = δ q ⊗ q ˉ , δ q ≈ [ 1 1 2 δ θ ] \mathbf{q} = \delta\mathbf{q} \otimes \bar{\mathbf{q}}, \quad \delta\mathbf{q} \approx \begin{bmatrix} 1 \\ \frac{1}{2}\delta\boldsymbol{\theta} \end{bmatrix} q=δq⊗qˉ,δq≈[121δθ]

其余状态：

v = v ˉ + δ v , p = p ˉ + δ p , b g = b ˉ g + δ b g , ⋯ \mathbf{v} = \bar{\mathbf{v}} + \delta\mathbf{v}, \quad \mathbf{p} = \bar{\mathbf{p}} + \delta\mathbf{p}, \quad \mathbf{b}_g = \bar{\mathbf{b}}_g + \delta\mathbf{b}_g, \quad \cdots v=vˉ+δv,p=pˉ+δp,bg=bˉg+δbg,⋯

1.3 坐标系

NED：北-东-下，惯性坐标系（地球坐标系）
FRD：前-右-下，机体坐标系
旋转矩阵 R ( q ) ∈ S O ( 3 ) \mathbf{R}(\mathbf{q}) \in SO(3) R(q)∈SO(3)：FRD → \to → NED
重力向量： g = [ 0 , 0 , g ] T \mathbf{g} = [0, 0, g]^T g=[0,0,g]T， g ≈ 9.80665 m/s 2 g \approx 9.80665 \text{ m/s}^2 g≈9.80665 m/s2

2. 标称状态动力学

标称状态由 IMU 积分驱动。设 IMU 采样间隔为 Δ t \Delta t Δt，陀螺仪测量 ω ∈ R 3 \boldsymbol{\omega} \in \mathbb{R}^3 ω∈R3（机体坐标系，rad/s），加速度计测量 a ∈ R 3 \mathbf{a} \in \mathbb{R}^3 a∈R3（机体坐标系，m/s²）。

2.1 姿态积分

补偿偏置后的角速度：

ω ˉ = ω − b ˉ g \bar{\boldsymbol{\omega}} = \boldsymbol{\omega} - \bar{\mathbf{b}}_g ωˉ=ω−bˉg

增量旋转（小角度近似）：

δ θ ω = ω ˉ ⋅ Δ t \delta\boldsymbol{\theta}_{\omega} = \bar{\boldsymbol{\omega}} \cdot \Delta t δθω=ωˉ⋅Δt

标称四元数更新（右乘）：

q ˉ k + 1 = q ˉ k ⊗ exp ⁡ ( 1 2 δ θ ω ) ≈ q ˉ k ⊗ [ 1 1 2 δ θ ω ] \bar{\mathbf{q}}_{k+1} = \bar{\mathbf{q}}k \otimes \exp\left(\frac{1}{2}\delta\boldsymbol{\theta}{\omega}\right) \approx \bar{\mathbf{q}}k \otimes \begin{bmatrix} 1 \\ \frac{1}{2}\delta\boldsymbol{\theta}{\omega} \end{bmatrix} qˉk+1=qˉk⊗exp(21δθω)≈qˉk⊗[121δθω]

2.2 速度积分

补偿偏置后的比力：

a ˉ b o d y = a − b ˉ a \bar{\mathbf{a}}_{body} = \mathbf{a} - \bar{\mathbf{b}}_a aˉbody=a−bˉa

旋转到 NED：

a ˉ n e d = R ˉ ⋅ a ˉ b o d y \bar{\mathbf{a}}{ned} = \bar{\mathbf{R}} \cdot \bar{\mathbf{a}}{body} aˉned=Rˉ⋅aˉbody

标称速度更新（梯形积分）：

v ˉ k + 1 = v ˉ k + ( a ˉ n e d + g ) ⋅ Δ t \bar{\mathbf{v}}_{k+1} = \bar{\mathbf{v}}k + \left( \bar{\mathbf{a}}{ned} + \mathbf{g} \right) \cdot \Delta t vˉk+1=vˉk+(aˉned+g)⋅Δt

注：精确实现还包含科里奥利加速度和传输率（地球自转和曲率效应），短距离飞行可忽略。

2.3 位置积分

p ˉ k + 1 = p ˉ k + 1 2 ( v ˉ k + v ˉ k + 1 ) ⋅ Δ t \bar{\mathbf{p}}_{k+1} = \bar{\mathbf{p}}_k + \frac{1}{2}\left( \bar{\mathbf{v}}k + \bar{\mathbf{v}}{k+1} \right) \cdot \Delta t pˉk+1=pˉk+21(vˉk+vˉk+1)⋅Δt

3. 误差状态传播：核心推导

3.1 推导策略

误差状态的传播方程定义为：

δ x k + 1 = x k + 1 true − x ˉ k + 1 \delta\mathbf{x}{k+1} = \mathbf{x}{k+1}^{\text{true}} - \bar{\mathbf{x}}_{k+1} δxk+1=xk+1true−xˉk+1

对于姿态，用四元数差分提取旋转矢量；对于其余状态，直接相减。然后在零误差处线性化，得到状态转移矩阵 F \mathbf{F} F 和噪声矩阵 G \mathbf{G} G。

3.2 姿态误差推导

真实状态运动学（含误差和噪声）：

真实角速度：

ω true = ω − ( b ˉ g + δ b g ) − ω n \boldsymbol{\omega}^{\text{true}} = \boldsymbol{\omega} - (\bar{\mathbf{b}}_g + \delta\mathbf{b}_g) - \boldsymbol{\omega}_n ωtrue=ω−(bˉg+δbg)−ωn

真实姿态预测：

q k + 1 true = q k true ⊗ exp ⁡ ( 1 2 ω true Δ t ) \mathbf{q}_{k+1}^{\text{true}} = \mathbf{q}_k^{\text{true}} \otimes \exp\left( \frac{1}{2} \boldsymbol{\omega}^{\text{true}} \Delta t \right) qk+1true=qktrue⊗exp(21ωtrueΔt)

代入真实状态 = 标称 + 误差：

q k + 1 true = ( δ q k ⊗ q ˉ k ) ⊗ exp ⁡ ( 1 2 ( ω ˉ − δ b g − ω n ) Δ t ) \mathbf{q}_{k+1}^{\text{true}} = \left( \delta\mathbf{q}_k \otimes \bar{\mathbf{q}}_k \right) \otimes \exp\left( \frac{1}{2} (\bar{\boldsymbol{\omega}} - \delta\mathbf{b}_g - \boldsymbol{\omega}_n) \Delta t \right) qk+1true=(δqk⊗qˉk)⊗exp(21(ωˉ−δbg−ωn)Δt)

标称状态运动学：

q ˉ k + 1 = q ˉ k ⊗ exp ⁡ ( 1 2 ω ˉ Δ t ) \bar{\mathbf{q}}_{k+1} = \bar{\mathbf{q}}_k \otimes \exp\left( \frac{1}{2} \bar{\boldsymbol{\omega}} \Delta t \right) qˉk+1=qˉk⊗exp(21ωˉΔt)

误差四元数（真实 ⊗ 标称逆）：

δ q k + 1 = q k + 1 true ⊗ q ˉ k + 1 − 1 \delta\mathbf{q}{k+1} = \mathbf{q}{k+1}^{\text{true}} \otimes \bar{\mathbf{q}}_{k+1}^{-1} δqk+1=qk+1true⊗qˉk+1−1

展开并化简（利用四元数乘法结合律和单位范数约束）：

δ q k + 1 = δ q k ⊗ q ˉ k ⊗ exp ⁡ ( 1 2 ( ω ˉ − δ b g − ω n ) Δ t ) ⊗ exp ⁡ ( − 1 2 ω ˉ Δ t ) ⊗ q ˉ k − 1 \delta\mathbf{q}_{k+1} = \delta\mathbf{q}_k \otimes \bar{\mathbf{q}}_k \otimes \exp\left( \frac{1}{2} (\bar{\boldsymbol{\omega}} - \delta\mathbf{b}_g - \boldsymbol{\omega}_n) \Delta t \right) \otimes \exp\left( -\frac{1}{2} \bar{\boldsymbol{\omega}} \Delta t \right) \otimes \bar{\mathbf{q}}_k^{-1} δqk+1=δqk⊗qˉk⊗exp(21(ωˉ−δbg−ωn)Δt)⊗exp(−21ωˉΔt)⊗qˉk−1

对于小角度，指数映射可线性化：

exp ⁡ ( 1 2 ( ω ˉ − δ b g − ω n ) Δ t ) ≈ exp ⁡ ( 1 2 ω ˉ Δ t ) ⊗ exp ⁡ ( 1 2 ( − δ b g − ω n ) Δ t ) \exp\left( \frac{1}{2} (\bar{\boldsymbol{\omega}} - \delta\mathbf{b}_g - \boldsymbol{\omega}_n) \Delta t \right) \approx \exp\left( \frac{1}{2} \bar{\boldsymbol{\omega}} \Delta t \right) \otimes \exp\left( \frac{1}{2} (-\delta\mathbf{b}_g - \boldsymbol{\omega}_n) \Delta t \right) exp(21(ωˉ−δbg−ωn)Δt)≈exp(21ωˉΔt)⊗exp(21(−δbg−ωn)Δt)

利用 exp ⁡ ( ϕ ) ⊗ exp ⁡ ( − ϕ ) = I \exp(\boldsymbol{\phi}) \otimes \exp(-\boldsymbol{\phi}) = \mathbf{I} exp(ϕ)⊗exp(−ϕ)=I：

δ q k + 1 ≈ δ q k ⊗ q ˉ k ⊗ exp ⁡ ( 1 2 ( − δ b g − ω n ) Δ t ) ⊗ q ˉ k − 1 \delta\mathbf{q}_{k+1} \approx \delta\mathbf{q}_k \otimes \bar{\mathbf{q}}_k \otimes \exp\left( \frac{1}{2} (-\delta\mathbf{b}_g - \boldsymbol{\omega}_n) \Delta t \right) \otimes \bar{\mathbf{q}}_k^{-1} δqk+1≈δqk⊗qˉk⊗exp(21(−δbg−ωn)Δt)⊗qˉk−1

这里 q ˉ k ⊗ exp ⁡ ( ⋅ ) ⊗ q ˉ k − 1 \bar{\mathbf{q}}_k \otimes \exp(\cdot) \otimes \bar{\mathbf{q}}_k^{-1} qˉk⊗exp(⋅)⊗qˉk−1 是相似变换 ，将机体坐标系下的旋转矢量转到 NED 坐标系。但在 EKF2 中，姿态误差定义在机体坐标系（local frame），所以相似变换后仍保留原形式。

提取旋转矢量（虚部 × 2）：

δ θ k + 1 = δ θ k − ( δ b g , k + ω n ) ⋅ Δ t \delta\boldsymbol{\theta}_{k+1} = \delta\boldsymbol{\theta}k - (\delta\mathbf{b}{g,k} + \boldsymbol{\omega}_n) \cdot \Delta t δθk+1=δθk−(δbg,k+ωn)⋅Δt

物理意义：

δ θ k \delta\boldsymbol{\theta}_k δθk：上一时刻的姿态误差直接传播
− δ b g , k ⋅ Δ t -\delta\mathbf{b}_{g,k} \cdot \Delta t −δbg,k⋅Δt：陀螺偏置误差导致的额外旋转
− ω n ⋅ Δ t -\boldsymbol{\omega}_n \cdot \Delta t −ωn⋅Δt：陀螺测量噪声导致的随机游走

简化假设：推导中忽略了 ( Δ t ) 2 (\Delta t)^2 (Δt)2 项（高阶小量），并利用四元数单位范数 q 0 2 + q 1 2 + q 2 2 + q 3 2 = 1 q_0^2 + q_1^2 + q_2^2 + q_3^2 = 1 q02+q12+q22+q32=1 化简。

3.3 速度误差推导

真实速度：

v k + 1 true = v ˉ k + δ v k + ( R ( q k true ) ⋅ ( a − b ˉ a − δ b a , k − a n ) + g ) Δ t \mathbf{v}_{k+1}^{\text{true}} = \bar{\mathbf{v}}_k + \delta\mathbf{v}_k + \left( \mathbf{R}(\mathbf{q}_k^{\text{true}}) \cdot (\mathbf{a} - \bar{\mathbf{b}}a - \delta\mathbf{b}{a,k} - \mathbf{a}_n) + \mathbf{g} \right) \Delta t vk+1true=vˉk+δvk+(R(qktrue)⋅(a−bˉa−δba,k−an)+g)Δt

标称速度：

v ˉ k + 1 = v ˉ k + ( R ˉ ⋅ ( a − b ˉ a ) + g ) Δ t \bar{\mathbf{v}}_{k+1} = \bar{\mathbf{v}}_k + \left( \bar{\mathbf{R}} \cdot (\mathbf{a} - \bar{\mathbf{b}}_a) + \mathbf{g} \right) \Delta t vˉk+1=vˉk+(Rˉ⋅(a−bˉa)+g)Δt

速度误差 = 真实 - 标称：

δ v k + 1 = δ v k + ( R ( q k true ) ⋅ ( a − b ˉ a − δ b a , k − a n ) − R ˉ ⋅ ( a − b ˉ a ) ) Δ t \delta\mathbf{v}_{k+1} = \delta\mathbf{v}_k + \left( \mathbf{R}(\mathbf{q}_k^{\text{true}}) \cdot (\mathbf{a} - \bar{\mathbf{b}}a - \delta\mathbf{b}{a,k} - \mathbf{a}_n) - \bar{\mathbf{R}} \cdot (\mathbf{a} - \bar{\mathbf{b}}_a) \right) \Delta t δvk+1=δvk+(R(qktrue)⋅(a−bˉa−δba,k−an)−Rˉ⋅(a−bˉa))Δt

关键步骤：线性化旋转矩阵。设真实姿态 = 标称姿态 ⊗ 小误差旋转：

R ( q true ) = R ( δ q ⊗ q ˉ ) = R ( δ q ) ⋅ R ˉ \mathbf{R}(\mathbf{q}^{\text{true}}) = \mathbf{R}(\delta\mathbf{q} \otimes \bar{\mathbf{q}}) = \mathbf{R}(\delta\mathbf{q}) \cdot \bar{\mathbf{R}} R(qtrue)=R(δq⊗qˉ)=R(δq)⋅Rˉ

小角度下：

R ( δ q ) ≈ I − [ δ θ ] × \mathbf{R}(\delta\mathbf{q}) \approx \mathbf{I} - [\delta\boldsymbol{\theta}]_\times R(δq)≈I−[δθ]×

其中 [ ⋅ ] × [\cdot]_\times [⋅]× 是叉乘矩阵（skew-symmetric）：

即 delta angle 噪声方差 = ( σ ω Δ t ) 2 (\sigma_\omega \Delta t)^2 (σωΔt)2，delta velocity 噪声方差 = ( σ a Δ t ) 2 (\sigma_a \Delta t)^2 (σaΔt)2。

两条路线的等价关系

令两种模型等价：

( σ ω Δ t ) 2 = q ω Δ t ⟹ σ ω 2 = q ω Δ t ⟹ σ ω = σ ARW Δ t (\sigma_\omega \Delta t)^2 = q_\omega \Delta t \implies \sigma_\omega^2 = \frac{q_\omega}{\Delta t} \implies \sigma_\omega = \frac{\sigma_{\text{ARW}}}{\sqrt{\Delta t}} (σωΔt)2=qωΔt⟹σω2=Δtqω⟹σω=Δt σARW

当 Δ t = 0.01 \Delta t = 0.01 Δt=0.01s 时， σ ω = 10 ⋅ σ ARW \sigma_\omega = 10 \cdot \sigma_{\text{ARW}} σω=10⋅σARW。

验证：若 σ ARW = 0.0015 \sigma_{\text{ARW}} = 0.0015 σARW=0.0015 rad/√s（典型 MEMS），则 σ ω = 0.015 \sigma_\omega = 0.015 σω=0.015 rad/s，与 EKF2 默认值 ekf2_gyr_noise = 0.015 吻合。

结论：PX4 的 σ ω \sigma_\omega σω 不是物理噪声密度，而是为固定 Δ t \Delta t Δt 调参的"等效标准差"。

5.2 噪声矩阵 G \mathbf{G} G（15×6）

G = [ 0 3 × 3 − I 3 × 3 ⋅ Δ t − R ˉ ⋅ Δ t 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 ] \mathbf{G} = \begin{bmatrix} \mathbf{0}{3\times3} & -\mathbf{I}{3\times3} \cdot \Delta t \\ -\bar{\mathbf{R}} \cdot \Delta t & \mathbf{0}{3\times3} \\ \mathbf{0}{3\times3} & \mathbf{0}{3\times3} \\ \mathbf{0}{3\times3} & \mathbf{0}{3\times3} \\ \mathbf{0}{3\times3} & \mathbf{0}_{3\times3} \end{bmatrix} G= 03×3−Rˉ⋅Δt03×303×303×3−I3×3⋅Δt03×303×303×303×3

物理意义：

陀螺噪声 ω n \boldsymbol{\omega}_n ωn → 姿态误差（经 Δ t \Delta t Δt 积分）
加速度计噪声 a n \mathbf{a}_n an → 速度误差（经 R ˉ \bar{\mathbf{R}} Rˉ 旋转 + Δ t \Delta t Δt 积分）
偏置随机游走不在 G \mathbf{G} G 中（单独处理）

5.3 IMU 噪声协方差 Q u \mathbf{Q}_u Qu（6×6）

Q u = diag ( σ a 2 , σ a 2 , σ a 2 , σ ω 2 , σ ω 2 , σ ω 2 ) \mathbf{Q}u = \text{diag}(\sigma_a^2, \sigma_a^2, \sigma_a^2, \sigma\omega^2, \sigma_\omega^2, \sigma_\omega^2) Qu=diag(σa2,σa2,σa2,σω2,σω2,σω2)

σ a \sigma_a σa = ekf2_acc_noise [m/s²]
σ ω \sigma_\omega σω = ekf2_gyr_noise [rad/s]

5.4 偏置随机游走的噪声

偏置不通过 G \mathbf{G} G 建模，单独添加：

Δ P b g = ( σ b ˙ g Δ t ) 2 I , Δ P b a = ( σ b ˙ a Δ t ) 2 I \Delta \mathbf{P}{b_g} = (\sigma{\dot{b}g} \Delta t)^2 \mathbf{I}, \quad \Delta \mathbf{P}{b_a} = (\sigma_{\dot{b}_a} \Delta t)^2 \mathbf{I} ΔPbg=(σb˙gΔt)2I,ΔPba=(σb˙aΔt)2I

σ b ˙ g \sigma_{\dot{b}_g} σb˙g = ekf2_gyr_b_noise [rad/s²]
σ b ˙ a \sigma_{\dot{b}_a} σb˙a = ekf2_acc_b_noise [m/s³]

5.5 完整协方差传播方程

P k + 1 = F P k F T + G Q u G T + Q bias + Q mag + Q wind + Q terrain \mathbf{P}{k+1} = \mathbf{F} \mathbf{P}k \mathbf{F}^T + \mathbf{G} \mathbf{Q}u \mathbf{G}^T + \mathbf{Q}{\text{bias}} + \mathbf{Q}{\text{mag}} + \mathbf{Q}{\text{wind}} + \mathbf{Q}_{\text{terrain}} Pk+1=FPkFT+GQuGT+Qbias+Qmag+Qwind+Qterrain

6. 误差增长分析

6.1 纯 IMU 积分的误差发散

假设无外部观测，分析误差方差随时间的增长。以下同时给出经典 PSD 推导和 PX4 参数形式。

姿态误差方差

经典 PSD 形式：

P θ ( t ) = P θ ( 0 ) + q ω t + 1 3 q b g t 3 P_\theta(t) = P_\theta(0) + q_\omega t + \frac{1}{3} q_{bg} t^3 Pθ(t)=Pθ(0)+qωt+31qbgt3

代入 q ω = σ ω 2 / Δ t q_\omega = \sigma_\omega^2 / \Delta t qω=σω2/Δt 和 q b g = σ b ˙ g 2 / Δ t q_{bg} = \sigma_{\dot{b}_g}^2 / \Delta t qbg=σb˙g2/Δt：

P θ ( t ) = P θ ( 0 ) + σ ω 2 Δ t t + 1 3 σ b ˙ g 2 Δ t t 3 P_\theta(t) = P_\theta(0) + \frac{\sigma_\omega^2}{\Delta t} t + \frac{1}{3} \frac{\sigma_{\dot{b}_g}^2}{\Delta t} t^3 Pθ(t)=Pθ(0)+Δtσω2t+31Δtσb˙g2t3

来源	增长规律	物理含义
陀螺白噪声	∝ t \propto t ∝t	角度随机游走
陀螺偏置漂移	∝ t 3 \propto t^3 ∝t3	偏置积分导致的姿态二次发散

速度误差方差

经典 PSD 形式：

P v ( t ) ≈ 1 3 q a t 3 P_v(t) \approx \frac{1}{3} q_a t^3 Pv(t)≈31qat3

PX4 参数形式：

P v ( t ) ≈ 1 3 σ a 2 Δ t t 3 P_v(t) \approx \frac{1}{3} \frac{\sigma_a^2}{\Delta t} t^3 Pv(t)≈31Δtσa2t3

位置误差方差

经典 PSD 形式：

P p ( t ) ≈ 1 20 q a t 5 P_p(t) \approx \frac{1}{20} q_a t^5 Pp(t)≈201qat5

PX4 参数形式：

P p ( t ) ≈ 1 20 σ a 2 Δ t t 5 P_p(t) \approx \frac{1}{20} \frac{\sigma_a^2}{\Delta t} t^5 Pp(t)≈201Δtσa2t5

数值估计

消费级 MEMS 参数：

σ ARW = 0.0015 \sigma_{\text{ARW}} = 0.0015 σARW=0.0015 rad/√s → EKF2 σ ω = 0.015 \sigma_\omega = 0.015 σω=0.015 rad/s（ Δ t = 0.01 \Delta t = 0.01 Δt=0.01s）
σ RRW , g = 10 − 4 \sigma_{\text{RRW},g} = 10^{-4} σRRW,g=10−4 rad/s^(3/2) → EKF2 σ b ˙ g = 0.001 \sigma_{\dot{b}_g} = 0.001 σb˙g=0.001 rad/s²

代入经典 PSD 公式（ q ω = ( 0.0015 ) 2 = 2.25 × 10 − 6 q_\omega = (0.0015)^2 = 2.25 \times 10^{-6} qω=(0.0015)2=2.25×10−6 rad²/s， q b g = ( 10 − 4 ) 2 = 10 − 8 q_{bg} = (10^{-4})^2 = 10^{-8} qbg=(10−4)2=10−8 rad²/s³）：

时间	P θ P_\theta Pθ（rad²）	标准差
1 s	2.3 × 10 − 6 2.3 \times 10^{-6} 2.3×10−6	0.0015 0.0015 0.0015 rad ≈ 0.09 ° 0.09° 0.09°
10 s	2.5 × 10 − 5 2.5 \times 10^{-5} 2.5×10−5	0.005 0.005 0.005 rad ≈ 0.29 ° 0.29° 0.29°
60 s	8.6 × 10 − 4 8.6 \times 10^{-4} 8.6×10−4	0.029 0.029 0.029 rad ≈ 1.7 ° 1.7° 1.7°

注：表中数值使用经典 PSD 参数。若误将 EKF2 参数 σ ω = 0.015 \sigma_\omega = 0.015 σω=0.015 直接当作 ARW 代入，结果会放大 100 倍。

结论：纯 IMU 在 60 秒内姿态误差约 1.7°，位置误差数十米，必须依靠外部观测校正。

7. 实现简介：Python 符号计算库

7.1 为什么用符号计算

上述推导中，状态转移矩阵 F \mathbf{F} F（24×24）和噪声矩阵 G \mathbf{G} G（24×6）包含数百个非零元素。手写这些元素的表达式容易出错，且当状态维度变化（如增删磁场/风状态）时需要重写全部代码。

EKF2 的解决方案：用 Python + SymForce 自动符号推导，生成 C++ 代码。

7.2 代码位置

复制代码

src/modules/ekf2/EKF/python/ekf_derivation/
├── derivation.py          # 核心推导脚本
├── utils/
│   └── derivation_utils.py  # 代码生成工具
└── generated/             # 自动生成的 C++ 头文件
    ├── predict_covariance.h   # 协方差传播（~644 行，1810 次运算）
    ├── compute_hagl_h.h       # 激光雷达雅可比
    ├── compute_mag_*.h        # 磁力计相关
    └── ...                    # 共 26 个 .h 文件

7.3 推导流程

derivation.py 中的 predict_covariance() 函数封装了上述全部推导。以下是其概念性流程（非逐行代码对应）：

定义状态 ：用 SymForce 的 Values 和 Rot3 定义原始状态
定义误差状态 ：姿态误差用 3 维旋转矢量 δ θ \delta\boldsymbol{\theta} δθ，其余用加法
真实状态 = 标称 + 误差 ：四元数左乘误差 δ q ⊗ q ˉ \delta\mathbf{q} \otimes \bar{\mathbf{q}} δq⊗qˉ，其余 x ˉ + δ x \bar{x} + \delta x xˉ+δx
定义噪声 ：加速度计白噪声 a n \mathbf{a}_n an、陀螺仪白噪声 ω n \boldsymbol{\omega}_n ωn
真实状态预测 ：用含噪声和误差的输入 ( a − b ˉ a − δ b a − a n ) (\mathbf{a} - \bar{\mathbf{b}}_a - \delta\mathbf{b}_a - \mathbf{a}_n) (a−bˉa−δba−an) 做 IMU 积分
标称状态预测 ：用不含噪声的输入 ( a − b ˉ a ) (\mathbf{a} - \bar{\mathbf{b}}_a) (a−bˉa) 做 IMU 积分
误差 = 真实 - 标称 ：对姿态用四元数差分 q true ⊗ q ˉ − 1 \mathbf{q}^{\text{true}} \otimes \bar{\mathbf{q}}^{-1} qtrue⊗qˉ−1 提取旋转矢量
自动求导 ：在零误差/零噪声处线性化
- F = ∂ δ x k + 1 ∂ δ x k \mathbf{F} = \frac{\partial \, \delta\mathbf{x}_{k+1}}{\partial \, \delta\mathbf{x}_k} F=∂δxk∂δxk+1
- G = ∂ δ x k + 1 ∂ n k \mathbf{G} = \frac{\partial \, \delta\mathbf{x}_{k+1}}{\partial \, \mathbf{n}_k} G=∂nk∂δxk+1
协方差传播 ： P n e w = F P F T + G Q u G T \mathbf{P}_{new} = \mathbf{F} \mathbf{P} \mathbf{F}^T + \mathbf{G} \mathbf{Q}_u \mathbf{G}^T Pnew=FPFT+GQuGT
代码生成 ：调用 generate_px4_function() 自动生成 C++ 头文件

7.4 关键技巧

零误差线性化 ：在 δ x = 0 \delta\mathbf{x} = \mathbf{0} δx=0 和 n = 0 \mathbf{n} = \mathbf{0} n=0 处求导，简化表达式
忽略 d t 2 dt^2 dt2：高阶小量直接设为零
四元数单位范数约束 ：利用 q 0 2 + q 1 2 + q 2 2 + q 3 2 = 1 q_0^2 + q_1^2 + q_2^2 + q_3^2 = 1 q02+q12+q22+q32=1 化简
只算上三角：利用协方差矩阵对称性，只生成上三角元素的计算

7.5 运行方式

bash 复制代码

cd src/modules/ekf2/EKF/python/ekf_derivation/
python3 derivation.py

可选参数：

--disable_mag：去掉磁场状态（18 维误差状态）
--disable_wind：去掉风速状态（22 维误差状态）

修改测量模型后，只需改 derivation.py 中的 Python 函数，重新运行脚本即可自动更新全部 C++ 代码。

8. 总结

推导对象	核心结果	物理意义
姿态误差	δ θ k + 1 = δ θ k − ( δ b g + ω n ) Δ t \delta\boldsymbol{\theta}_{k+1} = \delta\boldsymbol{\theta}_k - (\delta\mathbf{b}_g + \boldsymbol{\omega}_n)\Delta t δθk+1=δθk−(δbg+ωn)Δt	陀螺偏置和噪声导致姿态漂移
速度误差	δ v k + 1 = δ v k + ( [ a ˉ n e d ] × δ θ k − R ˉ δ b a − R ˉ a n ) Δ t \delta\mathbf{v}{k+1} = \delta\mathbf{v}k + ([\bar{\mathbf{a}}{ned}]\times \delta\boldsymbol{\theta}_k - \bar{\mathbf{R}}\delta\mathbf{b}_a - \bar{\mathbf{R}}\mathbf{a}_n)\Delta t δvk+1=δvk+([aˉned]×δθk−Rˉδba−Rˉan)Δt	姿态误差通过比力方程污染速度（核心耦合）
位置误差	δ p k + 1 = δ p k + δ v k Δ t \delta\mathbf{p}_{k+1} = \delta\mathbf{p}_k + \delta\mathbf{v}_k \Delta t δpk+1=δpk+δvkΔt	速度误差积分导致位置漂移
状态转移 F	15×15 稀疏矩阵，7 个非零子块	姿态→速度、速度→位置、偏置→姿态/速度
噪声矩阵 G	15×6 稀疏矩阵	加速度噪声→速度、陀螺噪声→姿态
协方差传播	P k + 1 = F P F T + G Q G T + Q bias \mathbf{P}{k+1} = \mathbf{F}\mathbf{P}\mathbf{F}^T + \mathbf{G}\mathbf{Q}\mathbf{G}^T + \mathbf{Q}{\text{bias}} Pk+1=FPFT+GQGT+Qbias	运动学传播 + IMU 噪声 + 偏置随机游走
纯 IMU 发散	位置 σ p ∝ t 5 / 2 \sigma_p \propto t^{5/2} σp∝t5/2	60 秒约 40 m，必须外部观测校正

关于我们：

灵智实验室（LingzhiLab）成立于2020年，核心团队源自西北工业大学，由一群深耕无人系统、自动控制与机器人技术的青年工程师与科研人员组成。我们始终秉持"开放、协同、智能、可靠"的理念，致力于推动无人智能体在复杂环境下的自主感知、决策与控制能力。

实验室聚焦于基于开源飞控（如PX4）与ROS 2的深度融合，构建高可靠、模块化、可扩展的无人系统软件架构。依托扎实的工程实践与学术背景，灵智实验室积极参与开源社区建设，助力科研教育与产业落地。