11. Mahony 互补滤波 AHRS 逐行注释¶

🧩 一句话：用陀螺做高频预测，用加计+磁力计做低频校正，PI 控制器把两者"互补"融合——只解姿态，不解位置。

💡 学习心态：这一章比 insupdate 简单——没有速度/位置更新，没有科里奥利/地球曲率，只关心姿态一个量。如果你看完了 03_insupdate，这里会觉得很轻松；如果你还没看 insupdate，从这里入门反而更容易。

📐 数学原理¶

1. 核心矛盾：陀螺 vs 加计+磁力计¶

互补滤波的核心思想：用陀螺做高频预测，用加计+磁力计做低频校正，两者在频域上"互补"。

2. 姿态误差的物理来源¶

假设真实姿态为 \(C_b^n\)，陀螺测得的角速度 \(\tilde{\boldsymbol{\omega}} = \boldsymbol{\omega} + \boldsymbol{\varepsilon}\)（含零偏 \(\boldsymbol{\varepsilon}\)）。

如果直接用陀螺积分：\(\mathbf{q}_{k+1} = \mathbf{q}_k \otimes \text{rv2q}(\tilde{\boldsymbol{\omega}} \cdot T_s)\)，零偏 \(\boldsymbol{\varepsilon}\) 会导致姿态误差线性发散。

怎么办？ 用加计和磁力计来"纠正"陀螺。

3. 加计误差推导——叉积怎么来的？¶

加计在 b 系测到的比力 \(\mathbf{f}^b\)，静态时 \(\mathbf{f}^b = -\mathbf{g}^b\)（重力在 b 系的投影）。

当前姿态矩阵 \(C_b^n\) 的第三行 \((C_b^n)_{3,:}\) 就是当前姿态认为的"天向"在 b 系的表示。

理论上，如果姿态完美，\(\mathbf{f}^b\) 应该和 \((C_b^n)_{3,:}\) 完全平行。

如果有姿态误差 \(\boldsymbol{\phi}\)，\(\mathbf{f}^b\) 和 \((C_b^n)_{3,:}\) 不平行，有一个小夹角。

用叉积来度量这个偏差：

\(\mathbf{e}_{\text{acc}} = (C_b^n)_{3,:}^T \times \hat{\mathbf{f}}^b\)

其中 \(\hat{\mathbf{f}}^b = \mathbf{f}^b / \|\mathbf{f}^b\|\) 是归一化后的加计测量。

为什么用叉积？ 两个单位向量的叉积 = \(\sin\theta\)（沿旋转轴方向），正好给出"把当前姿态往哪转才能对齐重力"的方向和大小。

数学推导（小角度近似）：

\(\mathbf{e}_{\text{acc}} = \begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix} \times \begin{bmatrix} f_x \\ f_y \\ f_z \end{bmatrix}_{\text{归一化}} = \begin{bmatrix} -f_y \\ f_x \\ 0 \end{bmatrix}\)

这正好对应 pitch 和 roll 的误差（不含 yaw）。

4. 磁力计误差推导¶

磁力计在 b 系测到 \(\mathbf{m}^b\)。理论参考磁场 \(\mathbf{w}^b\)（由 n 系参考磁场 \(C_b^n \mathbf{m}^b\) 投影回来）：

\(\mathbf{e}_{\text{mag}} = \mathbf{w}^b \times \hat{\mathbf{m}}^b\)

为什么磁力计只修正航向？ PSINS 在 MahonyUpdate.m L39 做了关键一步——把 n 系磁场的东向分量清零：

\(\mathbf{b}^n = \begin{bmatrix} 0 \\ \|\mathbf{m}^n_{\text{水平}}\| \\ m^n_z \end{bmatrix}\)

这样参考磁场只包含北向和垂向，不含东向 → 叉积误差只在航向上有分量，不会污染 pitch/roll。

5. PI 控制器设计¶

把加计误差 + 磁力计误差加起来：

\(\mathbf{e} = \mathbf{e}_{\text{acc}} + \mathbf{e}_{\text{mag}}\)

用 PI 控制器生成陀螺修正量：

\(\boldsymbol{\varepsilon}_{\text{反馈}} = K_p \cdot \mathbf{e} + K_i \int_0^t \mathbf{e} \, d\tau\)

• P 部分 \(K_p \cdot \mathbf{e}\)：比例修正，快速消除当前误差
• I 部分 \(K_i \int \mathbf{e}\,dt\)：积分修正，消除恒定陀螺零偏（零偏是常数 → 误差恒定 → 积分不断增长 → 抵消零偏）

6. 传递函数推导¶

MahonyInit 的注释里写了一个传递函数：

\(\delta\mathbf{q}(s) = \frac{sK_p + K_i}{s^2 + K_p s + K_i} \cdot \mathbf{f}_b(s) + \frac{s}{s^2 + K_p s + K_i} \cdot \mathbf{w}_b(s)\)

怎么来的？

系统方程（简化为一阶）：

• \(\dot{\boldsymbol{\phi}} = -\boldsymbol{\varepsilon}_{\text{反馈}} + \mathbf{w}_b\)（误差动力学，\(\boldsymbol{\phi}\) 是姿态误差角）
• \(\boldsymbol{\varepsilon}_{\text{反馈}} = K_p \mathbf{e} + K_i \int \mathbf{e}\,dt\)
• \(\mathbf{e} \approx \boldsymbol{\phi}\)（小角度下，叉积误差 ≈ 姿态误差角）

合并得：

\(\ddot{\boldsymbol{\phi}} + K_p \dot{\boldsymbol{\phi}} + K_i \boldsymbol{\phi} = \dot{\mathbf{w}}_b\)

拉普拉斯变换：

\(s^2 \boldsymbol{\phi}(s) + K_p s \boldsymbol{\phi}(s) + K_i \boldsymbol{\phi}(s) = s \cdot \mathbf{w}_b(s)\)

\(\frac{\boldsymbol{\phi}(s)}{\mathbf{w}_b(s)} = \frac{s}{s^2 + K_p s + K_i}\)

这是从陀螺噪声 \(\mathbf{w}_b\) 到姿态误差 \(\boldsymbol{\phi}\) 的传递函数——高通（陀螺高频噪声通过，低频被滤掉）。

同理，从加计误差（低频参考）到姿态误差的传递函数：

\(\frac{\boldsymbol{\phi}(s)}{\mathbf{f}_b(s)} = \frac{sK_p + K_i}{s^2 + K_p s + K_i}\)

这是低通（加计低频信号通过，高频被滤掉）。

7. 临界阻尼设计——\(K_p\) 和 \(K_i\) 怎么取？¶

分母 \(s^2 + K_p s + K_i\) 要设计成临界阻尼（既不过冲也不振荡）：

\(s^2 + K_p s + K_i = s^2 + 2\beta s + \beta^2 = (s + \beta)^2\)

所以：

• \(K_p = 2\beta\)
• \(K_i = \beta^2\)
• \(\beta = 1/\tau\)，\(\tau\) 是时间常数

PSINS 的经验系数：\(\beta = 2.146/\tau\)（2.146 是严老师调的经验值，比理论 \(1/\tau\) 略大）。

\(\tau\) 的物理意义：系统从误差衰减到 63% 所需的时间。\(\tau=4\)s 意味着姿态误差每 4 秒衰减到原来的 37%。

8. 最终姿态更新公式¶

\(\boxed{\mathbf{q}_{k+1} = \mathbf{q}_k \otimes \text{rv2q}\left(\boldsymbol{\phi}_m - \boldsymbol{\varepsilon}_{\text{反馈}} \cdot T_s\right)}\)

• \(\boldsymbol{\phi}_m\)：陀螺角增量（经圆锥补偿）
• \(\boldsymbol{\varepsilon}_{\text{反馈}} \cdot T_s\)：PI 反馈的修正量
• 两者相减 = "陀螺说转了这么多，但 PI 说要修正一些" → 净转动量

如果加计和陀螺完全一致（静态）：误差 \(\mathbf{e} \approx 0\)，\(\boldsymbol{\varepsilon}_{\text{反馈}} \approx 0\)，退化为纯陀螺积分。

如果加计检测到偏差：\(\mathbf{e} \neq 0\)，\(\boldsymbol{\varepsilon}_{\text{反馈}} \neq 0\)，把陀螺增量往加计方向"拉"。

9. rv2q 是什么？——旋转向量转四元数¶

公式里反复出现的 rv2q 是 rotation vector to quaternion 的缩写——把旋转向量转换为四元数。

旋转向量 \(\boldsymbol{\phi}\)：方向 = 旋转轴，大小 = 旋转角（rad）。

转换公式：

其中 \(\vert\boldsymbol{\phi}\vert\) 是旋转向量的模（即旋转角度）。

举例：绕 z 轴转 10° → \(\boldsymbol{\phi} = [0, 0, 10° \times \frac{\pi}{180}]^T\) → \(\text{rv2q}(\boldsymbol{\phi})\) 给出对应的四元数。

为什么用 rv2q 而不是直接四元数微分？ rv2q 是精确的大角度转换，没有小角度近似。在 insupdate L46 和 MahonyUpdate L49 都用它做姿态更新：先算出"这一步要转多少"（旋转向量），再转成四元数做乘法。比 \(\dot{q} = \frac{1}{2}q \otimes \omega\) 的一阶欧拉法精度更高。

📝 逐行注释¶

MahonyInit.m 逐行注释¶

🧩 函数作用：初始化 Mahony AHRS 结构体，设置 PI 控制器参数。

源码：MahonyInit.m

💡 关键理解：\(K_p\) 和 \(K_i\) 不是随便取的。\(K_p = 2\beta\)，\(K_i = \beta^2\) 保证系统是临界阻尼——既不过冲也不缓慢收敛。\(\tau\) 越大 → \(\beta\) 越小 → 滤波越平滑但响应越慢；\(\tau\) 越小 → 响应快但噪声大。

数值例：\(\tau=4\)s 时，\(\beta = 2.146/4 = 0.5365\)，\(K_p = 2 \times 0.5365 = 1.073\)，\(K_i = 0.5365^2 = 0.2878\)。姿态误差每 4 秒衰减到 37%。

MahonyUpdate.m 逐行注释¶

🧩 函数作用：一步 Mahony AHRS 更新——输入 IMU+磁力计，输出更新后的姿态。

源码：MahonyUpdate.m

L49 是整个 Mahony 的灵魂：phim - eb*nts = 陀螺测的角增量减去 PI 反馈的修正量。

如果加计和陀螺完全一致（静态），误差 \(\mathbf{e} \approx \mathbf{0}\)，\(\boldsymbol{\varepsilon}_{\text{反馈}} \approx \mathbf{0}\)，退化为纯陀螺积分；

如果加计检测到偏差，\(\boldsymbol{\varepsilon}_{\text{反馈}} \neq \mathbf{0}\)，把陀螺增量往加计方向"拉"。

§10 自适应模式详解（L20-35）¶

自适应的核心思想：加速度越偏离重力 = 越动态 = 越不信任加计 = \(\tau\) 越大

为什么不能直接 \(K_p, K_i = 0\)？ 因为需要平滑过渡。interplim 做线性插值，避免突变。

test_AHRS_Mahony.m 逐行注释¶

源码：test_AHRS_Mahony.m

这个 test 在做什么：模拟一个 pitch=1° 的静态场景，陀螺=0（不转），加计输出 [1,0,0.0175]（重力在 b 系的投影，pitch=1°）。Mahony 会从 \(\mathbf{q}=[1,0,0,0]\)（姿态=0）收敛到 pitch=1°。图里能看到指数收敛过程，收敛时间 \(\approx \tau = 4\)s。

🔍 断点调试建议¶

调试1：误差收敛验证¶

在 MATLAB 里跑 test_AHRS_Mahony.m，在 MahonyUpdate L41 设断点：

% 命中断点后
exyz  % 看误差向量，应该随时间衰减
ahrs.exyzInt  % 看积分项，应该收敛到稳态值
ahrs.Kp, ahrs.Ki  % 看 PI 参数

% 手算误差（对应 L41）
acc_man = [0; 1/glv.deg; 0] / norm([0; 1/glv.deg; 0]);  % 加计归一化
e_acc_man = cros(ahrs.Cnb(3,:)', acc_man);  % 重力方向叉积
norm(exyz - e_acc_man)  % <1e-14 说明加计部分理解对了

调试2：PI 反馈量验证¶

在 MahonyUpdate L49 设断点：

% KF 更新前
eb  % PI 反馈量 = Kp*exyz + exyzInt
phim  % 陀螺角增量
phim - eb*nts  % 净转动量

% 手算（对应 L49）
eb_man = ahrs.Kp*exyz + ahrs.exyzInt;
q_man = qmul(ahrs.q, rv2q(phim - eb_man*nts));
norm(ahrs.q - q_man)  % <1e-14 说明理解对了

调试3：自适应参数观察¶

用 MahonyInit(0.05)（自适应模式），在 MahonyUpdate L34 设断点：

nm1  % 加速度偏离 9.8 的程度
tau  % 当前自适应 tau
ahrs.Kp, ahrs.Ki  % 当前 PI 参数
% 静态时 nm1≈0, tau≈3, Kp≈1.4, Ki≈0.5
% 模拟加速时 nm1>0.1, tau→300000, Kp≈0, Ki≈0

❌ 初学者最容易踩的坑¶

1. 磁力计没归一化：L37 做了 mag = mag/nm，你自己移植时容易忘。不归一化 → 叉积大小受磁场强度影响 → PI 参数无效
2. 加计归一化忘了判 nm>0：如果加计输出 [0,0,0]（自由落体），除以 0 会 NaN。L18 的 if nm>0 是保护
3. Mahony 的 \(\tau\) 选太大：\(\tau=10\)s 看着稳，但加了 10° pitch 要 30s 才收敛。飞控场景 \(\tau\) 建议 1-3s
4. 磁力计补偿忘了去东向：L39 bxyz(1:2)=[0;norm(...)] 是把磁场水平投影到北向。不做这步 → 磁倾角会污染 roll/pitch
5. PI 的 I 项不抗饱和：如果长时间大加速度，积分项会不断增大。PSINS 的自适应模式通过增大 \(\tau\) 来间接减小 \(K_i\)，但嵌入式上可能需要加抗饱和限幅
6. 四元数没归一化：MahonyUpdate 没有显式 qnormlz！但 qmul+rv2q 在小角度下近似保持归一化。嵌入式移植时建议加上 q = q / norm(q)

🎯 配套练习¶

练习1：收敛时间验证¶

跑 test_AHRS_Mahony.m，回答：

• pitch 从 0° 收敛到 1° 的 63%（0.63°）需要多少秒？应该 \(\approx \tau = 4\)s
• 改 \(\tau=1\) 再跑一次，收敛快了多少？PI 参数变成多少？
• 改 \(\tau=10\) 再跑一次，收敛慢了多少？

练习2：自适应 vs 固定参数¶

用 MahonyInit(0.05)（自适应模式）跑 test_AHRS_Mahony.m，对比固定参数版：

• 自适应模式下 \(K_p/K_i\) 怎么随加速度变化？
• 静态时 \(\tau \approx\) 多少？动态时 \(\tau \approx\) 多少？

练习3：误差分解¶

在 MahonyUpdate L41 断点处，分别打印加计误差和磁力计误差：

e_acc = cros(ahrs.Cnb(3,:)', acc)
e_mag = cros(wxyz, mag)
exyz = e_acc + e_mag

• 静态时哪个分量为主？
• 如果去掉磁力计（mag=0），哪个分量消失？

练习4：嵌入式移植热身¶

把 MahonyUpdate.m 翻译成 C 伪代码（不需要编译），重点关注：

• L41 的叉积 cros(a,b) 怎么写？（提示：\(\mathbf{a} \times \mathbf{b} = [a_y b_z - a_z b_y, a_z b_x - a_x b_z, a_x b_y - a_y b_x]^T\)）
• L49 的 qmul 和 rv2q 需要哪些子函数？（提示：看 03_insupdate.md 附录 A.3）
• 自适应部分（L20-35）在嵌入式上可以简化为什么？（提示：查表法或简单 if-else）