PX4状态估计技术EKF2详解（六）：EKF2 磁力计融合——从航向修正到 3D 姿态约束

磁力计是 EKF2 中最容易被误解的传感器。很多人以为它只是个"电子罗盘"，用来告诉无人机北在哪。实际上，EKF2 的磁力计融合远比这复杂------它同时涉及航向、tilt、磁偏置、磁偏角、地磁模型，以及起飞后远离地面干扰的磁对齐机制。本文从四种 EKF2_MAG_TYPE 模式出发，讲清楚磁力计在 EKF2 中到底做了什么、没做什么，以及什么时候可以干脆不用它。

第 7 章（attitude_estimator_q 的两个问题讨论）已经揭示了一个关键事实：水平姿态与航向在直接通道上理论解耦，但间接耦合无法避免 。磁力计校正通过 rotateVector 和 rotateVectorInverse 与水平姿态紧密缠绕，而统一的 _gyro_bias 积分器进一步放大了这种耦合。本文将在此基础上，从 SO(3) 李群理论出发证明这些耦合不是代码缺陷，而是向量观测在旋转群上的内禀性质------并解释 EKF2 的 heading-only 模式为什么只能在卡尔曼增益层面"截断"耦合，却无法在观测层面"消除"耦合。

---

1. 磁力计在 EKF2 中的真实角色

磁力计测量的是机体坐标系下的磁场向量：

mmeas=RNEDbody⋅mearth+bmag \mathbf{m}{\text{meas}} = \mathbf{R}{\text{NED}}^{\text{body}} \cdot \mathbf{m}{\text{earth}} + \mathbf{b}{\text{mag}} mmeas=RNEDbody⋅mearth+bmag

• mearth\mathbf{m}_{\text{earth}}mearth：地磁场向量（NED 坐标系下，由世界地磁模型 WMM 给出）
• RNEDbody\mathbf{R}_{\text{NED}}^{\text{body}}RNEDbody：从 NED 到机体的旋转矩阵------这正是 EKF2 要估计的姿态
• bmag\mathbf{b}_{\text{mag}}bmag：机体磁场偏置（硬铁效应、电机电流干扰等）

关键洞察 ：磁力计不是直接测航向，而是测磁场方向。EKF2 知道当地的理论地磁场方向（通过 GPS 位置查询 WMM），把理论方向与实测方向对比，差值中包含了姿态误差的信息。这个差值可以用来修正：

• 航向（yaw）：最直接、最可靠
• tilt（roll/pitch）：仅在 3D 融合模式下有效
• 磁偏置 bmag\mathbf{b}_{\text{mag}}bmag：需要机体旋转才能观测

---

2. 磁力计观测的数学模型

EKF2 的状态向量中包含两个与磁力计相关的分量：

• mI∈R3\mathbf{m}_I \in \mathbb{R}^3mI∈R3：NED 坐标系下的地磁场向量（inertial frame，随地理位置变化）
• mB∈R3\mathbf{m}_B \in \mathbb{R}^3mB∈R3：机体坐标系下的磁力计偏置（body frame，硬铁效应 + 电机干扰）

2.1 预测测量

将 NED 坐标系下的地磁场 mI\mathbf{m}_ImI 转到机体坐标系，加上偏置 mB\mathbf{m}_BmB，得到预测的机体磁场：

m^body=q−1⊗mI+mB \hat{\mathbf{m}}_{\text{body}} = \mathbf{q}^{-1} \otimes \mathbf{m}_I + \mathbf{m}_B m^body=q−1⊗mI+mB

其中 q−1⊗mI\mathbf{q}^{-1} \otimes \mathbf{m}Iq−1⊗mI 表示用四元数的逆旋转 NED 向量到机体坐标系（等同于 RNEDbody⋅mI\mathbf{R}{\text{NED}}^{\text{body}} \cdot \mathbf{m}_IRNEDbody⋅mI）。

SymForce 自动生成代码 （python/ekf_derivation/derivation.py:416-421）：

def predict_mag_body(state) -> sf.V3:
    mag_field_earth = state["mag_I"]
    mag_bias_body = state["mag_B"]
    mag_body = state["quat_nominal"].inverse() * mag_field_earth + mag_bias_body
    return mag_body

2.2 创新计算

预测测量与实测磁场的差值即创新：

r=m^body−mmeas=q−1⊗mI+mB−mmeas \mathbf{r} = \hat{\mathbf{m}}{\text{body}} - \mathbf{m}{\text{meas}} = \mathbf{q}^{-1} \otimes \mathbf{m}_I + \mathbf{m}B - \mathbf{m}{\text{meas}} r=m^body−mmeas=q−1⊗mI+mB−mmeas

源码对应 （mag_fusion.cpp:74-88）：

aid_src.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index)
                                + _state.mag_B(index) - mag(index);

创新方向约定 ：源码中创新定义为 预测减实测 （predict - meas），而标准教材通常写 实测减预测 （meas - predict）。两种定义只差一个负号，EKF2 在 fuse() 中通过状态更新的符号完成等价：

x←x−K⋅r\mathbf{x} \leftarrow \mathbf{x} - \mathbf{K} \cdot \mathbf{r}x←x−K⋅r

代入 r=pred−meas\mathbf{r} = \text{pred} - \text{meas}r=pred−meas：

x−K(pred−meas)=x+K(meas−pred)\mathbf{x} - \mathbf{K}(\text{pred} - \text{meas}) = \mathbf{x} + \mathbf{K}(\text{meas} - \text{pred})x−K(pred−meas)=x+K(meas−pred)

最终效果与标准 EKF 的 x+K(meas−pred)\mathbf{x} + \mathbf{K}(\text{meas} - \text{pred})x+K(meas−pred) 完全一致。

2.3 雅可比矩阵：地磁场与偏置如何被修正

创新对误差状态 的雅可比 H\mathbf{H}H 决定了磁力计信息流向哪些状态分量。注意关键词------误差状态（tangent space，24 维），不是标称状态的存储形式（storage，25 维）。

SymForce 的 jacobian_chain_rule() 通过链式法则自动完成这一转换（derivation.py:221-249）。核心分解：

H=∂h∂xt∣xt=x^⏟=Jh⋅∂(x^⊕δx)∂δx∣δx=0⏟=J⊕ \mathbf{H} = \underbrace{\left.\frac{\partial h}{\partial \mathbf{x}t}\right|{\mathbf{x}t = \hat{\mathbf{x}}}}{=\mathbf{J}h} \cdot \underbrace{\left.\frac{\partial (\hat{\mathbf{x}} \oplus \delta\mathbf{x})}{\partial \delta\mathbf{x}}\right|{\delta\mathbf{x}=0}}{=\mathbf{J}\oplus} H==Jh ∂xt∂h xt=x^⋅=J⊕ ∂δx∂(x^⊕δx) δx=0

• Jh=∂h∂xt∣xt=x^\mathbf{J}_h = \frac{\partial h}{\partial \mathbf{x}t}|{\mathbf{x}_t = \hat{\mathbf{x}}}Jh=∂xt∂h∣xt=x^：观测函数对完整状态 的偏导，在标称状态处求值（tangent_space=False，1×25）
• J⊕=∂(x^⊕δx)∂δx∣δx=0\mathbf{J}\oplus = \frac{\partial (\hat{\mathbf{x}} \oplus \delta\mathbf{x})}{\partial \delta\mathbf{x}}|{\delta\mathbf{x}=0}J⊕=∂δx∂(x^⊕δx)∣δx=0：复合运算对误差状态 的偏导，在误差为零处求值（25×24）
  • 四元数块（4×3）：q⊕δθ=q⊗exp⁡(δθ/2)\mathbf{q} \oplus \delta\boldsymbol{\theta} = \mathbf{q} \otimes \exp(\delta\boldsymbol{\theta}/2)q⊕δθ=q⊗exp(δθ/2) 的线性化
  • 向量状态块：直接映射，恒等
• H=Jh⋅J⊕\mathbf{H} = \mathbf{J}h \cdot \mathbf{J}\oplusH=Jh⋅J⊕（1×24）：创新对误差状态的雅可比，与 24×24 协方差矩阵维度匹配

对地磁场误差 δmI\delta\mathbf{m}_IδmI 的雅可比：

地磁场 mI\mathbf{m}_ImI 是向量状态，存储空间与切空间相同（都是 R3\mathbb{R}^3R3），dx_derror 对应块为单位矩阵。因此：

∂r∂δmI=∂(m^body−mmeas)∂mI=RNEDbody \frac{\partial \mathbf{r}}{\partial \delta\mathbf{m}I} = \frac{\partial (\hat{\mathbf{m}}{\text{body}} - \mathbf{m}_{\text{meas}})}{\partial \mathbf{m}I} = \mathbf{R}{\text{NED}}^{\text{body}} ∂δmI∂r=∂mI∂(m^body−mmeas)=RNEDbody

对磁力计偏置误差 δmB\delta\mathbf{m}_BδmB 的雅可比：

同理，偏置也是向量状态：

∂r∂δmB=∂(m^body−mmeas)∂mB=I3×3 \frac{\partial \mathbf{r}}{\partial \delta\mathbf{m}B} = \frac{\partial (\hat{\mathbf{m}}{\text{body}} - \mathbf{m}_{\text{meas}})}{\partial \mathbf{m}B} = \mathbf{I}{3 \times 3} ∂δmB∂r=∂mB∂(m^body−mmeas)=I3×3

对姿态误差 δθ\delta\boldsymbol{\theta}δθ 的雅可比：

姿态是四元数（4 维存储，3 维切空间）。jacobian_chain_rule() 自动处理了四元数参数化到旋转向量的映射：

∂r∂δθ=∂(q−1⊗mI)∂δθ∣q=qˉ \frac{\partial \mathbf{r}}{\partial \delta\boldsymbol{\theta}} = \frac{\partial (\mathbf{q}^{-1} \otimes \mathbf{m}I)}{\partial \delta\boldsymbol{\theta}} \Bigg|{\mathbf{q} = \bar{\mathbf{q}}} ∂δθ∂r=∂δθ∂(q−1⊗mI) q=qˉ

这是磁场方向误差到姿态误差旋转向量 的映射，SymForce 自动生成（compute_mag_innov_innov_var_and_hx.h）。它是 3D 融合能修正 tilt 的数学基础。

为什么强调"误差状态"？ 因为在 ESKF 框架下，协方差矩阵 P\mathbf{P}P 始终描述误差状态的分布（24×24），卡尔曼增益 K=PHT(HPHT+R)−1\mathbf{K} = \mathbf{P}\mathbf{H}^T(\mathbf{H}\mathbf{P}\mathbf{H}^T + \mathbf{R})^{-1}K=PHT(HPHT+R)−1 中的 H\mathbf{H}H 必须是创新对误差状态的雅可比，否则维度不匹配。

2.4 顺序融合与数值稳定

磁力计三轴不是同时融合，而是顺序融合（sequential fusion）：

for (uint8_t index = 0; index <= 2; index++) {
    // x 轴先融合
    // y 轴再融合（用更新后的状态重新计算创新）
    // z 轴最后（可能跳过 synthetic mag z）
    measurementUpdate(Kfusion, H, R, innovation[index]);
}

每融合一轴后，状态向量（包括姿态、地磁场、偏置）被更新，下一轴的创新基于更新后的状态重新计算。这保证了三轴融合的顺序一致性，但要求协方差矩阵在每一步都保持正定------如果 innovation_variance < R_MAG，说明协方差数值退化，会触发 resetMagEarthCov() 和 resetMagBiasCov()。

2.5 Heading-only 模式如何实现解耦

当 update_tilt = false 时（Heading-only 模式），EKF2 不解耦测量方程，而是直接在卡尔曼增益层面做截断：

if (!update_tilt) {
    Kfusion(State::quat_nominal.idx + 0) = 0.f;  // roll 增益置零
    Kfusion(State::quat_nominal.idx + 1) = 0.f;  // pitch 增益置零
    Kfusion(State::gyro_bias.idx + 0) = 0.f;     // roll 陀螺偏置增益置零
    Kfusion(State::gyro_bias.idx + 1) = 0.f;     // pitch 陀螺偏置增益置零
}

雅可比 H\mathbf{H}H 本身仍然包含 tilt 分量，但增益 K\mathbf{K}K 的 tilt 通道被强制置零，因此磁力计创新不会驱动 roll/pitch 的更新。姿态误差中仅 yaw 分量被修正。

2.6 SO(3) 上的观测解耦：为什么误差不可能"天然正交"

第 7 章（attitude_estimator_q 的两个问题讨论）提出了一个深刻观察：水平姿态与航向在直接通道上理论解耦，但间接耦合无法避免 。互补滤波中，k × a 的第三行不含偏航角 ψ\psiψ，重力观测"理论上"只影响 roll/pitch；磁力计校正通过 atan2 提取偏航角，"理论上"只影响 yaw。但 attitude_estimator_q 的源码分析揭示了两条间接耦合路径：

1. 磁力计 → 水平姿态 ：_q.rotateVector(_mag) 把机体系磁场转到导航系时，roll/pitch 误差混入 mag_earth 的水平分量，mag_err 经 rotateVectorInverse 映射回机体系后分解到三轴；
2. 统一 _gyro_bias 积分器 ：重力误差和磁力计误差混合在同一个 corr 中，通过统一积分器互相污染。

本节从 SO(3) 李群理论出发，证明这些耦合不是代码实现的缺陷，而是向量观测在旋转群上的内禀性质------在 SO(3) 上，不存在两个"天然正交"的向量观测。

向量观测的雅可比矩阵

姿态 R∈SO(3)R \in SO(3)R∈SO(3)，向量观测模型为：

h(R)=RTv,v∈R3 h(R) = R^T v, \quad v \in \mathbb{R}^3 h(R)=RTv,v∈R3

对姿态做微小扰动 R=exp⁡([δθ]×)RˉR = \exp([\delta\theta]\times) \bar{R}R=exp([δθ]×)Rˉ，[⋅]×[\cdot]\times[⋅]× 为叉乘矩阵。线性化后：

∂h∂θ=−[RTv]× \frac{\partial h}{\partial \theta} = -[R^T v]_\times ∂θ∂h=−[RTv]×

关键结构 ：观测雅可比是叉乘矩阵 。对于任意向量 a=RTva = R^T va=RTv，−[a]×-[a]_\times−[a]× 的列为：

−[a]×=[0a3−a2−a30a1a2−a10] -[a]_\times = \begin{bmatrix} 0 & a_3 & -a_2 \\ -a_3 & 0 & a_1 \\ a_2 & -a_1 & 0 \end{bmatrix} −[a]×= 0−a3a2a30−a1−a2a10

三列分别对应 roll、pitch、yaw 方向的灵敏度。除非 aaa 的某个分量为零，否则三列全部非零。

重力观测 vs 磁力计观测

重力观测 ：vg=[0,0,g]Tv_g = [0, 0, g]^Tvg=[0,0,g]T（NED 下的重力向量）

在恒等姿态 R=IR = IR=I：

Hg=−[[0,0,g]T]×=[0−g0g00000] H_g = -[[0, 0, g]^T]_\times = \begin{bmatrix} 0 & -g & 0 \\ g & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix} Hg=−[[0,0,g]T]×= 0g0−g00000

第三列（yaw 方向）为零。这是 attitude_estimator_q 中"理论解耦"的来源------在恒等姿态的线性化近似下，重力观测不直接驱动 yaw。

磁力计观测 ：vm=[mx,my,mz]Tv_m = [m_x, m_y, m_z]^Tvm=[mx,my,mz]T（NED 下地磁场）

在恒等姿态 R=IR = IR=I：

Hm=−[[mx,my,mz]T]×=[0−mzmymz0−mx−mymx0] H_m = -[[m_x, m_y, m_z]^T]_\times = \begin{bmatrix} 0 & -m_z & m_y \\ m_z & 0 & -m_x \\ -m_y & m_x & 0 \end{bmatrix} Hm=−[[mx,my,mz]T]×= 0mz−my−mz0mxmy−mx0

三列全部非零（假设地磁场三个分量都不为零）。

正交性判据与不可能性证明

两个观测的误差方向在切空间 TISO(3)≅so(3)T_I SO(3) \cong \mathfrak{so}(3)TISO(3)≅so(3) 中的重叠程度，由 H1TH2H_1^T H_2H1TH2 衡量。若 H1TH2=0H_1^T H_2 = 0H1TH2=0，则误差方向严格正交。

设 a=RTv1a = R^T v_1a=RTv1，b=RTv2b = R^T v_2b=RTv2，则：

H1TH2=[a]×T[b]×=−[a]×[b]× H_1^T H_2 = [a]\times^T [b]\times = -[a]\times [b]\times H1TH2=[a]×T[b]×=−[a]×[b]×

利用叉乘矩阵恒等式 [a]×[b]×=baT−(aTb)I[a]\times [b]\times = ba^T - (a^T b)I[a]×[b]×=baT−(aTb)I：

H1TH2=−baT+(aTb)I=(aTb)I−baT H_1^T H_2 = -ba^T + (a^T b)I = (a^T b)I - ba^T H1TH2=−baT+(aTb)I=(aTb)I−baT

baTba^TbaT 是秩 1 矩阵（外积），(aTb)I(a^T b)I(aTb)I 是标量乘以单位矩阵 。一个秩 1 矩阵不可能等于一个满秩的对角矩阵，除非 aTb=0a^T b = 0aTb=0 且 baT=0ba^T = 0baT=0 同时成立------但后者要求 a=0a = 0a=0 或 b=0b = 0b=0，即观测向量本身为零。

结论 ：对于任意两个非零向量观测 v1,v2v_1, v_2v1,v2，在任意姿态 R∈SO(3)R \in SO(3)R∈SO(3)：

H1TH2≠0 H_1^T H_2 \neq \mathbf{0} H1TH2=0

这意味着向量观测的误差方向在切空间中永远有非零重叠，不可能构造出"天然正交"的观测对。

"线性化解耦"只是局部近似

重力观测在 R=IR = IR=I 时 yaw 列为零，但这只是特定线性化点的巧合：

Hg∣R=I⋅e3=0,e3=[0,0,1]T H_g|_{R=I} \cdot \mathbf{e}_3 = 0, \quad \mathbf{e}_3 = [0, 0, 1]^T Hg∣R=I⋅e3=0,e3=[0,0,1]T

然而，这个性质并不限于恒等姿态 。设姿态用 ZYX 欧拉角表示 R=Rz(ψ)Ry(θ)Rx(ϕ)R = R_z(\psi) R_y(\theta) R_x(\phi)R=Rz(ψ)Ry(θ)Rx(ϕ)，则：

RTvg=Rx(−ϕ)Ry(−θ)Rz(−ψ)[0,0,−g]T⏟= [0,0,−g]T=[gsin⁡θgsin⁡ϕcos⁡θ−gcos⁡ϕcos⁡θ] R^T v_g = R_x(-\phi) R_y(-\theta) \underbrace{R_z(-\psi) [0, 0, -g]^T}_{=\,[0,0,-g]^T} = \begin{bmatrix} g\sin\theta \\ g\sin\phi\cos\theta \\ -g\cos\phi\cos\theta \end{bmatrix} RTvg=Rx(−ϕ)Ry(−θ)=[0,0,−g]T Rz(−ψ)[0,0,−g]T= gsinθgsinϕcosθ−gcosϕcosθ

Hg=−[RTvg]×H_g = -[R^T v_g]_\timesHg=−[RTvg]× 的第三列（yaw 方向）为 [−gsin⁡ϕcos⁡θ,  gsin⁡θ,  0]T[-g\sin\phi\cos\theta,\; g\sin\theta,\; 0]^T[−gsinϕcosθ,gsinθ,0]T。在常规飞行包线内（ϕ,θ∈(−π/2,π/2)\phi, \theta \in (-\pi/2, \pi/2)ϕ,θ∈(−π/2,π/2)），只要 roll=0 且 pitch=0，无论偏航角 ψ\psiψ 取何值，第三列恒为零：

ϕ=0,  θ=0  ⇒  Hg⋅e3=0,∀ψ∈[0,2π) \phi = 0, \; \theta = 0 \;\Rightarrow\; H_g \cdot \mathbf{e}_3 = \mathbf{0}, \quad \forall \psi \in [0, 2\pi) ϕ=0,θ=0⇒Hg⋅e3=0,∀ψ∈[0,2π)

物理直觉 ：当 roll=0、pitch=0 时，机体 z 轴与世界 z 轴对齐。绕机体 z 轴（偏航）旋转，重力在机体系中的投影始终是 [0,0,−g]T[0, 0, -g]^T[0,0,−g]T------加速度计"感觉"不到这个旋转，偏航角对重力观测完全不可观测。

重要区分 ：HgH_gHg 是对小姿态误差向量 δθ\delta\boldsymbol{\theta}δθ 的雅可比，不是对欧拉角偏航 ψ\psiψ 的直接偏导。这两者不是一回事。

从物理量直接求导更清楚。重力在世界系中为 g=[0,0,g]T\mathbf{g} = [0, 0, g]^Tg=[0,0,g]T，加速度计测到的是 zg=RTg\mathbf{z}_g = R^T \mathbf{g}zg=RTg。用 ZYX 欧拉角 R=Rz(ψ)Ry(θ)Rx(ϕ)R = R_z(\psi) R_y(\theta) R_x(\phi)R=Rz(ψ)Ry(θ)Rx(ϕ)：

zg=RxT(ϕ)RyT(θ)RzT(ψ)g⏟= g \mathbf{z}g = R_x^T(\phi) R_y^T(\theta) \underbrace{R_z^T(\psi) \mathbf{g}}{=\,\mathbf{g}} zg=RxT(ϕ)RyT(θ)=g RzT(ψ)g

因为 g\mathbf{g}g 在世界 z 轴上，偏航旋转 Rz(ψ)R_z(\psi)Rz(ψ) 不改变它。因此：

∂(RTg)∂ψ=0 \frac{\partial (R^T \mathbf{g})}{\partial \psi} = \mathbf{0} ∂ψ∂(RTg)=0

物理结论：重力观测始终对绕世界竖直轴的旋转不敏感，无论机体是否倾斜。偏航角（绕世界 z 轴的旋转）对重力观测永远不可观测。

---

那 HgH_gHg 第三列非零意味着什么？Hg=−[RTg]×H_g = -[R^T \mathbf{g}]_\timesHg=−[RTg]× 的零空间为：

null(Hg)=span(RTg) \text{null}(H_g) = \text{span}(R^T \mathbf{g}) null(Hg)=span(RTg)

即沿着当前重力方向的旋转误差不可观测 。当 roll=0、pitch=0 时，RTg=[0,0,−g]TR^T \mathbf{g} = [0, 0, -g]^TRTg=[0,0,−g]T 与机体系 z 轴对齐，EKF 误差状态第三分量恰好对应偏航误差，所以第三列为零。一旦机体倾斜，RTgR^T \mathbf{g}RTg 偏离机体系 z 轴，此时误差状态第三分量不再纯粹是偏航误差，而是混入了其他方向的旋转成分------这就是第三列非零的参数化耦合来源，不等于重力突然能观测真实偏航角。

这正是磁力计不可替代的根本原因：重力观测只能约束与重力方向垂直的两个自由度，偏航角（绕世界竖直轴的旋转）始终不可观测。无论机体水平还是倾斜，可靠的偏航约束始终需要磁力计（或其他外部航向参考如 EV、GPS 航迹）。

这也解释了 attitude_estimator_q 中的间接耦合路径 A：横滚误差通过 rotateVector 混入磁力计校正，本质上是误差参数化在非恒等姿态下产生的耦合，而非物理上重力获得了偏航观测能力。

Heading-only 模式的理论本质

既然观测不可能天然正交，heading-only 模式就不是"解耦观测"，而是在卡尔曼增益层面人为截断耦合通道：

K=[00Kyaw⋮],Kroll=Kpitch=0 \mathbf{K} = \begin{bmatrix} 0 \\ 0 \\ K_{\text{yaw}} \\ \vdots \end{bmatrix}, \quad \mathbf{K}{\text{roll}} = \mathbf{K}{\text{pitch}} = 0 K= 00Kyaw⋮ ,Kroll=Kpitch=0

测量方程中的耦合仍然存在（H\mathbf{H}H 的 tilt 分量非零），但更新量 Δx=Kr\Delta\mathbf{x} = \mathbf{K}\mathbf{r}Δx=Kr 的 tilt 分量被强制置零。这是增益层面的解耦 ，不是观测层面的解耦。

这与 attitude_estimator_q 中分离式 PI 结构的思路一致------Mahony 用两套独立积分器隔离误差，EKF2 用零增益截断更新。两者都是工程妥协：既然内禀耦合无法消除，就在信息流动路径上设置闸门。

代价 ：损失了磁力计对 tilt 的修正信息。在 3D 融合模式下，磁力计通过 HmH_mHm 的 roll/pitch 列提供额外的 tilt 约束；heading-only 模式下这些约束被丢弃，tilt 精度依赖加速度计和陀螺仪。

收益：磁干扰无法通过耦合通道污染 roll/pitch，这正是磁干扰环境下保护姿态精度的关键。

---

3. 四种融合策略：EKF2_MAG_TYPE

PX4 提供四种磁力计融合策略，通过参数 EKF2_MAG_TYPE 设置：

模式	值	对 tilt 的影响	对 heading 的影响	使用时机
Automatic	0	起飞后修正	始终修正	默认，户外飞行
Magnetic heading	1	从不修正	始终修正	磁干扰环境，保护 tilt
None	5	无	无	磁环境完全不可信
Init only	6	仅初始化	仅初始化	起飞前可信、起飞后干扰

2.1 Automatic（默认）

Automatic 模式是最复杂的策略------它在不同阶段自动切换融合方式：

阶段 A：地面/起飞前（!mag_aligned_in_flight）

• 使用 Heading-only 融合 （mag_hdg）
• 只修正 yaw，不修正 tilt
• 原因：地面附近磁场干扰大（钢筋、电机、电池），3D 融合会污染 roll/pitch

阶段 B：起飞后（mag_aligned_in_flight = true）

• 当离地高度 > 1.5 m 时，触发 checkHaglYawResetReq()，重置 yaw 并切换到 3D 融合 （mag_3D）
• 融合磁力计 x/y/z 三轴，同时修正 yaw 和 tilt
• 磁偏置 bmag\mathbf{b}_{\text{mag}}bmag 被持续估计

为什么起飞后才切换 3D？

地面附近的磁场环境通常与空中不同：

• 混凝土中的钢筋产生局部磁异常
• 起飞平台（金属桌、车辆）干扰磁场
• 电池大电流在启动瞬间产生强磁场

飞到 1.5 m 以上后，这些干扰显著减弱，磁力计数据更可靠，才允许用 3D 融合修正 tilt。

2.2 Magnetic Heading（推荐用于磁干扰环境）

与 Automatic 不同，Heading 模式从不修正 tilt：

if (!update_tilt) {
    Kfusion(State::quat_nominal.idx + 0) = 0.f;  // roll 增益置零
    Kfusion(State::quat_nominal.idx + 1) = 0.f;  // pitch 增益置零
    Kfusion(State::gyro_bias.idx + 0) = 0.f;     // roll 陀螺偏置增益置零
    Kfusion(State::gyro_bias.idx + 1) = 0.f;     // pitch 陀螺偏置增益置零
}

在 fuseMag() 中，通过把卡尔曼增益的 roll/pitch 分量强制置零，确保磁力计创新只影响 yaw 和磁状态（mag_I、mag_B）。

代价：tilt 误差无法被磁力计纠正。如果长时间没有 GPS/速度辅助，陀螺偏置会导致 roll/pitch 缓慢漂移。

2.3 None（完全不用磁力计）

磁力计数据被完全忽略。EKF2 依靠其他传感器维持航向：

• GPS 速度（固定翼巡航时有持续可观测性）
• 外部视觉（EV yaw）
• GPS 双天线航向（EKF2_GPS_CTRL 的 dual antenna heading）

注意：多旋翼悬停时 GPS 无法提供航向可观测性（水平加速度为零）。没有磁力计的多旋翼在悬停时 yaw 会纯靠陀螺积分漂移，几分钟内就可能偏几十度。

2.4 Init Only（起飞前用、起飞后不用）

磁力计仅在初始化阶段使用：

• 上电时用地磁场方向初始化 yaw
• 解锁（arm）后停止磁力计融合
• 此后航向靠 GPS 速度或外部视觉约束

典型场景 ：多旋翼在地面时磁力计数据干净，但起飞后电机大电流产生强磁场干扰。Init only 让无人机在起飞前获得正确航向，起飞后不再受电机磁场影响。

与 None 的关键区别：

• Init only：初始化时有航向，解锁后 GPS 直接可用 → 位置控制模式可以从起飞第一秒就运行
• None：没有初始化航向 → 需要等 GPS 积累足够加速度后才能确定航向 → 起飞后前几秒可能没有位置控制

---

4. 如何选择？决策树

根据 PX4 官方文档的选择树，可以按以下逻辑选择 EKF2_MAG_TYPE：

第一步：磁场环境是否稳定？

磁场环境稳定（户外开阔地、远离金属结构、电机电流小）：

• → 选择 Automatic（0） 或 Magnetic heading（1）

磁场环境不稳定（室内、高楼附近、大电流电机靠近磁力计）：

• → 进入第二步

第二步：是静态应用还是动态飞行？

静态应用（如地面机器人、固定安装的传感器）：

• → 磁场稳定 → Automatic/Heading
• → 磁场不稳定 → None，使用外部航向源（双天线 GPS、视觉）

动态飞行：

• → 进入第三步

第三步：起飞前磁场是否可信？

起飞前磁场可信（地面无干扰，起飞后才出现干扰，如电机启动后大电流）：

• → Init only（6）
• 起飞前用地磁场初始化航向，起飞后停止磁力计融合，改用 GPS/视觉约束航向

起飞前磁场也不可信（如放在金属桌上、室内钢筋环境中启动）：

• → 进入第四步

第四步：是否手动起飞？

手动起飞（操作员可以控制起飞方向，等待 GPS 航向初始化）：

• → None（5） + GPS 速度约束航向
• 起飞后需要足够的水平加速度才能让 GPS 确定航向

自动起飞（需要位置控制模式从起飞第一秒就运行）：

• → None 不可行，因为没有初始航向，位置控制无法运行
• → 考虑使用 双天线 GPS 航向 （EKF2_GPS_CTRL 的 dual antenna heading）
• 或者改善硬件（外置磁力计远离干扰源）后用 Init only

决策速查表

场景	推荐模式	替代方案
户外航拍，磁环境干净	Automatic	---
户外航拍，但担心偶发磁干扰	Magnetic heading	---
室内飞行（钢筋结构）	Init only / None	外部视觉（VICON/OptiTrack）
大电流电机靠近磁力计	Init only	外置磁计，远离电机
无磁环境（矿区、桥底）	None	双天线 GPS、视觉
固定翼长距离巡航（无磁）	None	GPS 速度约束航向
多旋翼悬停为主（无磁）	不推荐 None	必须配视觉/双天线 GPS

---

5. 磁偏置与磁偏角：两个不同的概念

4.1 磁偏置（Magnetic Bias，机体坐标系）

bmag=mmeas−RNEDbody⋅mearth \mathbf{b}{\text{mag}} = \mathbf{m}{\text{meas}} - \mathbf{R}{\text{NED}}^{\text{body}} \cdot \mathbf{m}{\text{earth}} bmag=mmeas−RNEDbody⋅mearth

磁偏置是机体坐标系下的恒定偏移，来源包括：

• 硬铁效应（机身内的永磁体、磁铁）
• 电机电流产生的磁场
• PCB 走线电流的磁场
• 电池大电流的磁场

可观测性条件 ：磁偏置只有在机体旋转时才可观测。

原因：旋转改变 RNEDbody\mathbf{R}{\text{NED}}^{\text{body}}RNEDbody，从而改变 R⋅mearth\mathbf{R} \cdot \mathbf{m}{\text{earth}}R⋅mearth 的方向，但 bmag\mathbf{b}_{\text{mag}}bmag 在机体坐标系下是恒定的。EKF2 通过对比不同姿态下的测量值，分离出恒定的偏置和变化的地磁场分量。如果无人机一直悬停不转，磁偏置和地磁场投影纠缠在一起，无法区分。

4.2 磁偏角（Magnetic Declination，NED 坐标系）

磁偏角是真北与磁北之间的夹角，随地理位置变化：

declination=arctan⁡(mearth,ymearth,x) \text{declination} = \arctan\left(\frac{m_{\text{earth},y}}{m_{\text{earth},x}}\right) declination=arctan(mearth,xmearth,y)

EKF2 使用 WMM（World Magnetic Model） 根据 GPS 位置查询当地的磁偏角。当没有 GPS/EV 等 NE 辅助时，EKF2 会融合磁偏角作为观测，防止航向长期漂移：

if (no_ne_aiding_or_not_moving) {
    fuseDeclination(wmm_declination_rad, R_DECL, update_all_states, update_tilt);
}

• R_DECL = 0.5² = 0.25 rad²（约 14° 的标准差）
• 磁偏角融合非常宽松------WMM 模型本身有误差，且磁偏角随时间缓慢变化
• 它的作用不是精确约束航向，而是防止航向发散到完全错误的方向

---

6. 起飞后的磁对齐：远离地面干扰

5.1 为什么地面磁场不可靠

地面附近的磁场环境比空中复杂得多：

• 混凝土中的钢筋产生局部磁异常（可达地磁场的 50~200%）
• 金属桌、车辆、建筑结构改变磁场方向
• 起飞平台的电子设备产生干扰

5.2 HAGL Yaw Reset 机制

if (_control_status.flags.in_air && _control_status.flags.yaw_align
    && !_control_status.flags.mag_aligned_in_flight) {
    
    static constexpr float mag_anomalies_max_hagl = 1.5f;  // 1.5 米
    
    if (terrain_height > mag_anomalies_max_hagl) {
        // 重置 yaw，摆脱地面磁干扰
        resetMagStates(_mag_lpf.getState(), true);
        _control_status.flags.mag_aligned_in_flight = true;
    }
}

当无人机起飞并离地高度超过 1.5 m 时：

1. EKF2 认为已经远离地面磁异常
2. 用当前磁力计低通滤波值重置磁状态
3. 设置 mag_aligned_in_flight = true
4. Automatic 模式从此切换到 3D 融合（修正 tilt）

这个设计解释了为什么不要在地面旋转无人机来校准磁力计------地面磁场本身可能就是错的，校准出来的偏置在空中反而不准确。

---

7. 无磁环境下的航向维持：GPS 与视觉

当 EKF2_MAG_TYPE = None 时，EKF2 必须依靠其他传感器维持航向。

6.1 GPS 速度约束航向

GPS 速度提供航向可观测性的条件是：

aIMUNED≈v˙GPS≠0 \mathbf{a}{\text{IMU}}^{\text{NED}} \approx \dot{\mathbf{v}}{\text{GPS}} \neq 0 aIMUNED≈v˙GPS=0

即必须有非零的水平加速度。

机型	巡航状态	水平加速度	航向可观测性
固定翼	持续前飞	气动阻力产生持续向后的加速度	✅ 强
固定翼	协调转弯	持续向心加速度	✅ 强
多旋翼	悬停	零	❌ 无
多旋翼	直线匀速	零	❌ 无
多旋翼	加速/减速	有	⚠️ 弱（短暂）
多旋翼	原地旋转	零（位置不变）	❌ 无

关键结论：

• 固定翼没有磁力计也能长时间飞行------巡航时持续有可观测性
• 多旋翼没有磁力计时，悬停/匀速飞行阶段航向完全不可观测，纯靠陀螺积分漂移
• 多旋翼使用 None 模式时，必须有外部视觉 或双天线 GPS 作为替代航向源

6.2 双天线 GPS 航向

如果 GPS 模块支持双天线（如 u-blox ZED-F9P），可以通过两个天线的相对位置直接测量航向：

// gnss_yaw_control.cpp
float obs = getEulerYaw(ev_sample.quat);  // 类似 EV yaw

双天线 GPS 航向不受磁场影响，是矿区、桥底等无磁环境的首选替代方案。

6.3 外部视觉（VICON / OptiTrack / 视觉 SLAM）

外部视觉提供完整的 6DoF 位姿，包括航向。在 indoor 飞行中，这是磁力计的最佳替代。

---

8. 常见故障与排查

7.1 "Mag 3D fusion failing" 警告

原因：磁力计创新持续超出门限，test_ratio > 1。

排查：

1. 检查磁力计校准是否过期（SENS_MAG_MODE 是否为自动校准）
2. 检查飞行环境是否有强磁干扰（高压线、金属结构）
3. 检查电机电流是否过大（大电流靠近磁力计）
4. 尝试将 EKF2_MAG_TYPE 从 Automatic 改为 Magnetic heading

7.2 起飞后 yaw 突然跳变

原因：HAGL yaw reset 触发，地面磁干扰导致初始化 yaw 错误，起飞后 1.5m 处用空中磁场修正。

解决：

1. 起飞前远离金属结构
2. 起飞后不要急转，给 EKF2 时间重新对齐
3. 如果频繁跳变，考虑使用 Init only 或 Magnetic heading

7.3 悬停时缓慢自旋（yaw drift）

原因：陀螺偏置导致 yaw 缓慢漂移，磁力计融合停止（可能因故障被抑制）或根本没有磁力计。

排查：

1. 检查 estimator_status 中的 mag_hdg 或 mag_3D 标志是否为 true
2. 检查磁力计数据是否有效（mag.isAllFinite()）
3. 如果是 None 模式，检查 GPS 是否有足够加速度提供航向约束

7.4 roll/pitch 被磁力计污染

原因：在磁干扰环境中使用 Automatic 模式，3D 融合将错误的磁场方向当作 tilt 误差修正。

表现：无人机明明水平悬停，但姿态显示有微小倾斜；或者倾斜方向与磁场干扰方向相关。

解决 ：改用 Magnetic heading 模式。

---

9. 参数与调参

参数	含义	默认值	调参建议
EKF2_MAG_TYPE	磁力计融合模式	0 (Auto)	磁干扰环境改为 1
EKF2_MAG_NOISE	磁力计观测噪声	0.005 Gauss	磁干扰大时增大
EKF2_MAG_DECL	手动磁偏角	0	无 GPS 时手动设置当地值
EKF2_DECL_TYPE	磁偏角来源	7 (WMM+自动学习)	无网络时用保存值
EKF2_HDG_GATE	航向创新门限	2.6	磁跳变频繁时放宽
EKF2_SYNT_MAG_Z	合成 Z 轴磁场	false	Z 轴干扰大时开启

8.1 合成 Z 轴磁场

当磁力计 Z 轴受严重干扰（如电池电流沿 Z 轴分布）时，可以开启 EKF2_SYNT_MAG_Z：

mz,synth=sign(mz,pred)⋅mearth2−mx2−my2 m_{z,\text{synth}} = \text{sign}(m_{z,\text{pred}}) \cdot \sqrt{m_{\text{earth}}^2 - m_x^2 - m_y^2} mz,synth=sign(mz,pred)⋅mearth2−mx2−my2

EKF2 用理论地磁场强度和实测 X/Y 轴值，合成"理想"的 Z 轴值，忽略实际测量中的 Z 轴干扰。

---

10. 小结

磁力计融合的核心认知：

1. 磁力计不只是测航向------3D 融合模式下，它还修正 tilt。但这把双刃剑在磁干扰环境中会反噬 roll/pitch。
2. 磁偏置只在旋转时可观测------悬停不动的无人机无法校准磁偏置。
3. 起飞后的磁对齐很关键------1.5 m 高度处的 yaw reset 是摆脱地面磁干扰的关键设计。
4. 多旋翼离不开磁力计或替代航向源------GPS 速度在悬停时无法约束航向，无磁多旋翼会缓慢自旋。
5. 选择树的核心逻辑：磁环境稳定 → Auto/Heading；磁环境不稳定 → Init only / None + 替代航向源。

关于我们：

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

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