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$ × $\\cdot$ _\times $\cdot$ × 是叉乘矩阵（skew-symmetric）：

$ϕ$ × = $0 - ϕ z ϕ y ϕ z 0 - ϕ x - ϕ y ϕ x 0$ $\\boldsymbol{\\phi}$ _\times = \begin{bmatrix} 0 & -\phi_z & \phi_y \\ \phi_z & 0 & -\phi_x \\ -\phi_y & \phi_x & 0 \end{bmatrix} $ϕ$ ×= 0ϕz−ϕy−ϕz0ϕxϕy−ϕx0

代入：

R ( q true ) ≈ ( I − $δ θ$ × ) ⋅ R ˉ = R ˉ − $δ θ$ × ⋅ R ˉ \mathbf{R}(\mathbf{q}^{\text{true}}) \approx (\mathbf{I} - $\\delta\\boldsymbol{\\theta}$ \times) \cdot \bar{\mathbf{R}} = \bar{\mathbf{R}} - $\\delta\\boldsymbol{\\theta}$ \times \cdot \bar{\mathbf{R}} R(qtrue)≈(I− $δθ$ ×)⋅Rˉ=Rˉ− $δθ$ ×⋅Rˉ

代回速度误差：

δ v k + 1 = δ v k + ( ( R ˉ − $δ θ$ × ⋅ R ˉ ) ⋅ ( a − b ˉ a − δ b a − a n ) − R ˉ ⋅ ( a − b ˉ a ) ) Δ t = δ v k + ( − $δ θ$ × ⋅ R ˉ ⋅ ( a − b ˉ a ) − R ˉ ⋅ δ b a − R ˉ ⋅ a n ) Δ t + O ( δ 2 ) \begin{aligned} \delta\mathbf{v}_{k+1} &= \delta\mathbf{v}k + \left( (\bar{\mathbf{R}} - $\\delta\\boldsymbol{\\theta}$ \times \cdot \bar{\mathbf{R}}) \cdot (\mathbf{a} - \bar{\mathbf{b}}_a - \delta\mathbf{b}_a - \mathbf{a}_n) - \bar{\mathbf{R}} \cdot (\mathbf{a} - \bar{\mathbf{b}}_a) \right) \Delta t \\ &= \delta\mathbf{v}k + \left( - $\\delta\\boldsymbol{\\theta}$ \times \cdot \bar{\mathbf{R}} \cdot (\mathbf{a} - \bar{\mathbf{b}}_a) - \bar{\mathbf{R}} \cdot \delta\mathbf{b}_a - \bar{\mathbf{R}} \cdot \mathbf{a}_n \right) \Delta t + O(\delta^2) \end{aligned} δvk+1=δvk+((Rˉ− $δθ$ ×⋅Rˉ)⋅(a−bˉa−δba−an)−Rˉ⋅(a−bˉa))Δt=δvk+(− $δθ$ ×⋅Rˉ⋅(a−bˉa)−Rˉ⋅δba−Rˉ⋅an)Δt+O(δ2)

定义标称加速度（NED）：

a ˉ n e d = R ˉ ⋅ ( a − b ˉ a ) \bar{\mathbf{a}}_{ned} = \bar{\mathbf{R}} \cdot (\mathbf{a} - \bar{\mathbf{b}}_a) aˉned=Rˉ⋅(a−bˉa)

利用恒等式 − $ϕ$ × ⋅ a = $a$ × ⋅ ϕ - $\\boldsymbol{\\phi}$ \times \cdot \mathbf{a} = $\\mathbf{a}$ \times \cdot \boldsymbol{\phi} − $ϕ$ ×⋅a= $a$ ×⋅ϕ：

− $δ θ$ × ⋅ a ˉ n e d = $a ˉ n e d$ × ⋅ δ θ - $\\delta\\boldsymbol{\\theta}$ \times \cdot \bar{\mathbf{a}}{ned} = $\\bar{\\mathbf{a}}_{ned}$ _\times \cdot \delta\boldsymbol{\theta} − $δθ$ ×⋅aˉned= $aˉned$ ×⋅δθ

最终得到：

δ v k + 1 = δ v k + ( $a ˉ n e d$ × ⋅ δ θ k − R ˉ ⋅ δ b a , k − R ˉ ⋅ a n ) Δ t \delta\mathbf{v}_{k+1} = \delta\mathbf{v}k + \left( $\\bar{\\mathbf{a}}_{ned}$ \times \cdot \delta\boldsymbol{\theta}k - \bar{\mathbf{R}} \cdot \delta\mathbf{b}{a,k} - \bar{\mathbf{R}} \cdot \mathbf{a}_n \right) \Delta t δvk+1=δvk+( $aˉned$ ×⋅δθk−Rˉ⋅δba,k−Rˉ⋅an)Δt

物理意义：

δ v k \delta\mathbf{v}_k δvk：上一时刻速度误差直接传播
$a ˉ n e d$ × ⋅ δ θ k ⋅ Δ t $\\bar{\\mathbf{a}}_{ned}$ _\times \cdot \delta\boldsymbol{\theta}_k \cdot \Delta t $aˉned$ ×⋅δθk⋅Δt：姿态误差通过比力方程污染速度。这是统一解算的核心耦合！
− R ˉ ⋅ δ b a , k ⋅ Δ t -\bar{\mathbf{R}} \cdot \delta\mathbf{b}_{a,k} \cdot \Delta t −Rˉ⋅δba,k⋅Δt：加速度偏置误差
− R ˉ ⋅ a n ⋅ Δ t -\bar{\mathbf{R}} \cdot \mathbf{a}_n \cdot \Delta t −Rˉ⋅an⋅Δt：加速度计测量噪声

如果姿态有 1° 误差，加速度计读数旋转到 NED 时会引入约 0.017 ⋅ g ≈ 0.17 m/s 2 0.017 \cdot g \approx 0.17 \text{ m/s}^2 0.017⋅g≈0.17 m/s2 的水平加速度误差。10 ms 内导致约 0.0017 m/s 0.0017 \text{ m/s} 0.0017 m/s 的速度误差，1 秒内累积约 0.17 m/s 0.17 \text{ m/s} 0.17 m/s。这就是姿态精度对导航精度的基础性影响。

3.4 位置误差推导

δ p k + 1 = p k + 1 true − p ˉ k + 1 = ( p ˉ k + δ p k + ( v ˉ k + δ v k ) Δ t ) − ( p ˉ k + v ˉ k Δ t ) \delta\mathbf{p}{k+1} = \mathbf{p}{k+1}^{\text{true}} - \bar{\mathbf{p}}_{k+1} = (\bar{\mathbf{p}}_k + \delta\mathbf{p}_k + (\bar{\mathbf{v}}_k + \delta\mathbf{v}_k) \Delta t) - (\bar{\mathbf{p}}_k + \bar{\mathbf{v}}_k \Delta t) δpk+1=pk+1true−pˉk+1=(pˉk+δpk+(vˉk+δvk)Δt)−(pˉk+vˉkΔt)

δ p k + 1 = δ p k + δ v k ⋅ Δ t \delta\mathbf{p}_{k+1} = \delta\mathbf{p}_k + \delta\mathbf{v}_k \cdot \Delta t δpk+1=δpk+δvk⋅Δt

位置误差只来自速度误差的积分，没有直接的测量噪声项。

3.5 偏置误差推导

陀螺偏置和加速度偏置在 EKF2 中被建模为随机游走（Random Walk）。但随机游走的数学表述有两套并行的体系，理解它们的差异是避免量纲混乱的关键。

路线一：经典教材（从 PSD 出发）

连续时间下，偏置被建模为白噪声的积分：

b ˙ ( t ) = w ( t ) , E $w ( t ) w ( τ ) T$ = q b δ ( t − τ ) \dot{\mathbf{b}}(t) = \mathbf{w}(t), \quad E $\\mathbf{w}(t)\\mathbf{w}(\\tau)\^T$ = \mathbf{q}_b \delta(t-\tau) b˙(t)=w(t),E $w(t)w(τ)T$ =qbδ(t−τ)

其中 w ( t ) \mathbf{w}(t) w(t) 是白噪声过程， q b \mathbf{q}_b qb 是功率谱密度（PSD）。

量	单位（陀螺偏置）	单位（加速度偏置）
偏置 b \mathbf{b} b	rad/s	m/s²
白噪声 w = b ˙ \mathbf{w} = \dot{\mathbf{b}} w=b˙	rad/s²	m/s³
PSD q b \mathbf{q}_b qb	rad²/s³	m²/s⁵

离散化（一步增量）：

Δ b k = ∫ k Δ t ( k + 1 ) Δ t w ( τ ) d τ ∼ N ( 0 , q b Δ t ) \Delta \mathbf{b}k = \int{k\Delta t}^{(k+1)\Delta t} \mathbf{w}(\tau) d\tau \sim \mathcal{N}(\mathbf{0}, \mathbf{q}_b \Delta t) Δbk=∫kΔt(k+1)Δtw(τ)dτ∼N(0,qbΔt)

方差与步长线性成正比 ： Var ( Δ b k ) = q b Δ t \text{Var}(\Delta \mathbf{b}_k) = \mathbf{q}_b \Delta t Var(Δbk)=qbΔt。

定义 rate random walk（RRW）：

σ RRW = q b \sigma_{\text{RRW}} = \sqrt{q_b} σRRW=qb

单位：陀螺 σ RRW , g \sigma_{\text{RRW},g} σRRW,g = rad/s^(3/2)，加速度计 σ RRW , a \sigma_{\text{RRW},a} σRRW,a = m/s^(5/2)。

路线二：PX4 EKF2（从"每秒变化率"出发）

EKF2 不直接使用 PSD，而是定义了一个偏置变化率参数：

cpp 复制代码

float ekf2_gyr_b_noise{1.0e-3f};  // rad/sec**2
float ekf2_acc_b_noise{1.0e-2f};  // m/sec**3

这个参数被当作偏置导数的标准差 σ b ˙ \sigma_{\dot{b}} σb˙，离散化方式也不同：

Var ( Δ b k ) = ( σ b ˙ ⋅ Δ t ) 2 = σ b ˙ 2 ⋅ Δ t 2 \text{Var}(\Delta \mathbf{b}k) = (\sigma{\dot{b}} \cdot \Delta t)^2 = \sigma_{\dot{b}}^2 \cdot \Delta t^2 Var(Δbk)=(σb˙⋅Δt)2=σb˙2⋅Δt2

方差与步长平方成正比。

两条路线的等价关系

如果让两种离散化等价：

q b Δ t = σ b ˙ 2 Δ t 2 ⟹ q b = σ b ˙ 2 Δ t ⟹ σ RRW = σ b ˙ Δ t \mathbf{q}b \Delta t = \sigma{\dot{b}}^2 \Delta t^2 \implies \mathbf{q}b = \sigma{\dot{b}}^2 \Delta t \implies \sigma_{\text{RRW}} = \sigma_{\dot{b}} \sqrt{\Delta t} qbΔt=σb˙2Δt2⟹qb=σb˙2Δt⟹σRRW=σb˙Δt

这意味着 PX4 的 σ b ˙ \sigma_{\dot{b}} σb˙ 与经典 RRW 的关系：

σ b ˙ = σ RRW Δ t \sigma_{\dot{b}} = \frac{\sigma_{\text{RRW}}}{\sqrt{\Delta t}} σb˙=Δt σRRW

当 Δ t = 0.01 \Delta t = 0.01 Δt=0.01s 时， σ b ˙ = 10 ⋅ σ RRW \sigma_{\dot{b}} = 10 \cdot \sigma_{\text{RRW}} σb˙=10⋅σRRW。EKF2 的 ekf2_gyr_b_noise = 0.001 rad/s² 对应经典 RRW 约 10 − 4 10^{-4} 10−4 rad/s^(3/2)。

核心差异 ：经典路线用 PSD q b q_b qb（与 Δ t \Delta t Δt 无关的物理常数），PX4 路线用 σ b ˙ \sigma_{\dot{b}} σb˙（隐含 Δ t \Delta t Δt 依赖的调参常数）。由于 EKF2 的 Δ t \Delta t Δt 固定（0.01s），两者可以互相转换。

标称状态预测

无论哪条路线，标称偏置在预测步中都不变：

b ˉ k + 1 = b ˉ k \bar{\mathbf{b}}_{k+1} = \bar{\mathbf{b}}_k bˉk+1=bˉk

因为白噪声的期望为零： E $b ˙$ = E $w$ = 0 E $\\dot{\\mathbf{b}}$ = E $\\mathbf{w}$ = \mathbf{0} E $b˙$ =E $w$ =0。

误差状态传播

δ b k + 1 = δ b k \delta\mathbf{b}_{k+1} = \delta\mathbf{b}_k δbk+1=δbk

F 矩阵中偏置自块为单位矩阵，偏置误差不直接耦合其他状态。

协方差传播

路线	离散噪声方差	EKF2 代码
经典（PSD）	Q b b = q b Δ t Q_{bb} = q_b \Delta t Qbb=qbΔt	---
PX4（变化率）	Q b b = ( σ b ˙ Δ t ) 2 Q_{bb} = (\sigma_{\dot{b}} \Delta t)^2 Qbb=(σb˙Δt)2	`P(i,i) += sq(dt * ekf2_gyr_b_noise)`

EKF2 走路线二，这是工程近似------参数为固定 Δ t \Delta t Δt 调参。

与一阶高斯-马尔可夫过程的关系

随机游走可视为 FOGM 在相关时间 T → ∞ T \to \infty T→∞ 时的极限。FOGM 更精确（偏置自相关指数衰减），但需要估计额外参数 β = 1 / T \beta = 1/T β=1/T。EKF2 取随机游走极限以简化调参。

与源码的对应关系

模型层级	源码位置	偏置的行为
标称状态预测	`ekf.cpp:predictState()`	不变（常数）
误差状态传播	`predict_covariance.h`	δ b + = δ b \delta\mathbf{b}^+ = \delta\mathbf{b} δb+=δb
协方差传播	`covariance.cpp`	P b b + = P b b + Q b b P_{bb}^+ = P_{bb} + Q_{bb} Pbb+=Pbb+Qbb，手动加噪声

4. 状态转移矩阵 F \mathbf{F} F（24×24）

4.1 分块结构

将误差状态按 $δ θ , δ v , δ p , δ b g , δ b a$ $\\delta\\boldsymbol{\\theta}, \\delta\\mathbf{v}, \\delta\\mathbf{p}, \\delta\\mathbf{b}_g, \\delta\\mathbf{b}_a$ $δθ,δv,δp,δbg,δba$ 排列（前 15 维，导航核心）：

δ x k + 1 = F ⋅ δ x k + G ⋅ n k \delta\mathbf{x}_{k+1} = \mathbf{F} \cdot \delta\mathbf{x}_k + \mathbf{G} \cdot \mathbf{n}_k δxk+1=F⋅δxk+G⋅nk

F = $F θ θ 0 0 F θ b g 0 F v θ F v v 0 0 F v b a 0 F p v F p p 0 0 0 0 0 F b g b g 0 0 0 0 0 F b a b a$ \mathbf{F} = \begin{bmatrix} \mathbf{F}{\theta\theta} & \mathbf{0} & \mathbf{0} & \mathbf{F}{\theta b_g} & \mathbf{0} \\ \mathbf{F}{v\theta} & \mathbf{F}{vv} & \mathbf{0} & \mathbf{0} & \mathbf{F}{v b_a} \\ \mathbf{0} & \mathbf{F}{pv} & \mathbf{F}{pp} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{F}{b_g b_g} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{F}_{b_a b_a} \end{bmatrix} F= FθθFvθ0000FvvFpv0000Fpp00Fθbg00Fbgbg00Fvba00Fbaba

4.2 各子块详解

姿态-姿态：

F θ θ = I 3 × 3 \mathbf{F}{\theta\theta} = \mathbf{I}{3\times3} Fθθ=I3×3

姿态误差直接传播，无衰减。

姿态-陀螺偏置：

F θ b g = − I 3 × 3 ⋅ Δ t \mathbf{F}{\theta b_g} = -\mathbf{I}{3\times3} \cdot \Delta t Fθbg=−I3×3⋅Δt

陀螺偏置误差直接转化为姿态漂移。

速度-姿态（核心耦合块）：

F v θ = $a ˉ n e d$ × ⋅ Δ t \mathbf{F}{v\theta} = $\\bar{\\mathbf{a}}_{ned}$ \times \cdot \Delta t Fvθ= $aˉned$ ×⋅Δt

姿态误差通过比力方程污染速度。 $a ˉ n e d$ × $\\bar{\\mathbf{a}}_{ned}$ _\times $aˉned$ × 是标称加速度的叉乘矩阵。

速度-速度：

F v v = I 3 × 3 \mathbf{F}{vv} = \mathbf{I}{3\times3} Fvv=I3×3

速度-加速度偏置：

F v b a = − R ˉ ⋅ Δ t \mathbf{F}_{v b_a} = -\bar{\mathbf{R}} \cdot \Delta t Fvba=−Rˉ⋅Δt

加速度偏置经姿态旋转后污染速度。

位置-速度：

F p v = I 3 × 3 ⋅ Δ t \mathbf{F}{pv} = \mathbf{I}{3\times3} \cdot \Delta t Fpv=I3×3⋅Δt

位置-位置：

F p p = I 3 × 3 \mathbf{F}{pp} = \mathbf{I}{3\times3} Fpp=I3×3

偏置-偏置：

F b g b g = F b a b a = I 3 × 3 \mathbf{F}{b_g b_g} = \mathbf{F}{b_a b_a} = \mathbf{I}_{3\times3} Fbgbg=Fbaba=I3×3

4.3 稀疏性分析

15×15 导航核心矩阵中，非零子块只有 7 个（对角 5 个 + 非对角 2 个），其余 17 个子块为零。这种稀疏性使得协方差传播的计算复杂度从 O ( n 3 ) O(n^3) O(n3) 降低到接近 O ( n 2 ) O(n^2) O(n2)。

完整 24×24 矩阵还包括磁场、风速、地形的子块。磁场和风速建模为独立随机游走，与导航状态无直接耦合（F 中对应块为零），仅通过测量更新间接关联。

5. 噪声矩阵 G \mathbf{G} G 与噪声协方差 Q \mathbf{Q} Q

IMU 噪声的处理同样有两条并行路线。本节先对比，再给出 EKF2 的具体实现。

5.1 两条路线的定义对比

路线一：经典教材（PSD 模型）

连续时间 IMU 噪声为白噪声：

噪声	PSD	单位	常用参数
角速度噪声 ω n ( t ) \omega_n(t) ωn(t)	q ω q_\omega qω	rad²/s³	σ ARW = q ω \sigma_{\text{ARW}} = \sqrt{q_\omega} σARW=qω $rad/\sqrts$
加速度噪声 a n ( t ) a_n(t) an(t)	q a q_a qa	m²/s⁵	σ VRW = q a \sigma_{\text{VRW}} = \sqrt{q_a} σVRW=qa $m/s/\sqrts$

ARW（Angle Random Walk）和 VRW（Velocity Random Walk）是惯性导航的标准参数。

离散化：

采样保持（平均噪声）： Var ( w k ) = q / Δ t \text{Var}(w_k) = q / \Delta t Var(wk)=q/Δt
积分噪声（delta angle / delta velocity）： Var ( δ w k ) = q ⋅ Δ t \text{Var}(\delta w_k) = q \cdot \Delta t Var(δwk)=q⋅Δt

EKF2 的输入是 delta angle 和 delta velocity，属于积分噪声：

Var ( δ θ k ) = q ω Δ t = σ ARW 2 Δ t \text{Var}(\delta\theta_k) = q_\omega \Delta t = \sigma_{\text{ARW}}^2 \Delta t Var(δθk)=qωΔt=σARW2Δt
Var ( δ v k ) = q a Δ t = σ VRW 2 Δ t \text{Var}(\delta v_k) = q_a \Delta t = \sigma_{\text{VRW}}^2 \Delta t Var(δvk)=qaΔt=σVRW2Δt

路线二：PX4 EKF2（每秒变化率模型）

EKF2 定义：

cpp 复制代码

float ekf2_gyr_noise{1.5e-2f};   // rad/sec
float ekf2_acc_noise{3.5e-1f};   // m/sec**2

注释明确说是 IMU angular rate noise（rad/sec），即连续时间角速度噪声标准差 σ ω \sigma_\omega σω。

在 predict_covariance.h 中，噪声通过 G 矩阵进入误差状态。G 矩阵的元素已包含 Δ t \Delta t Δt：

姿态-陀螺元素： G θ ω ≈ − I ⋅ Δ t G_{\theta\omega} \approx -\mathbf{I} \cdot \Delta t Gθω≈−I⋅Δt
速度-加速度元素： G v a ≈ − R ˉ ⋅ Δ t G_{va} \approx -\bar{\mathbf{R}} \cdot \Delta t Gva≈−Rˉ⋅Δt

所以协方差传播中的噪声贡献：

G ⋅ Q u ⋅ G T ≈ Δ t 2 ⋅ $σ ω 2 I 0 0 σ a 2 I$ \mathbf{G} \cdot \mathbf{Q}u \cdot \mathbf{G}^T \approx \Delta t^2 \cdot \begin{bmatrix} \sigma\omega^2 \mathbf{I} & \mathbf{0} \\ \mathbf{0} & \sigma_a^2 \mathbf{I} \end{bmatrix} G⋅Qu⋅GT≈Δt2⋅ $σω2I00σa2I$

即 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 \times 3 - I 3 \times 3 \cdot Δ t - R ˉ \cdot Δ t 0 3 \times 3 0 3 \times 3 0 3 \times 3 0 3 \times 3 0 3 \times 3 0 3 \times 3 0 3 \times 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的深度融合，构建高可靠、模块化、可扩展的无人系统软件架构。依托扎实的工程实践与学术背景，灵智实验室积极参与开源社区建设，助力科研教育与产业落地。