12. QEKF 四元数扩展卡尔曼滤波 AHRS 逐行注释¶

🧩 一句话：用 7 维状态向量（四元数 4 + 陀螺零偏 3），通过 EKF 预测+量测更新，最优融合陀螺+加计+磁力计——比 Mahony 精度更高，因为有权重最优（协方差）融合。

💡 学习心态：如果你已经看了 09_kfupdate.md（卡尔曼滤波五公式），QEKF 就是它的一个具体应用——状态只有 7 维，量测只有 3 维，比 15 状态 SINS/GPS 组合简单得多。如果你还没看 09，也别怕，这里的 EKF 公式会从头推导。

📐 数学原理¶

1. 状态向量——为什么是 7 维？¶

为什么四元数是 4 维而不是 3 维？ 四元数有 4 个元素但只有 3 个自由度（因为 \(\|\mathbf{q}\|=1\) 约束）。用 4 维是因为四元数乘法没有奇异性（不像欧拉角有万向锁）。代价是 EKF 需要处理 4 维状态的冗余，归一化约束通过 L55 qnormlz 保证。

为什么陀螺零偏是 3 维？ 陀螺三轴各自有恒定零偏（假设短时间内不变），作为状态可以在线估计。这是 QEKF 相比 Mahony 的核心优势——Mahony 只能通过积分项隐含估计零偏，QEKF 直接把零偏作为状态显式估计。

2. 状态方程——四元数微分方程¶

四元数微分方程：

其中 \(\bar{\boldsymbol{\omega}} = [0, \boldsymbol{\omega}^T]^T\) 是角速度的纯四元数形式，\(\boldsymbol{\omega} = \tilde{\boldsymbol{\omega}} - \boldsymbol{\varepsilon}_b\)（测量值减零偏）。

展开成矩阵形式：

零偏状态方程：

（假设零偏在短时间内恒定不变）

3. 状态转移矩阵 \(\Phi\) 的推导¶

EKF 需要状态转移矩阵 \(\Phi = I + F \cdot T_s\)（一阶离散化），其中 \(F = \frac{\partial \dot{\mathbf{x}}}{\partial \mathbf{x}}\) 是雅可比矩阵。

对 \(\mathbf{q}\) 部分：

对 \(\boldsymbol{\varepsilon}_b\) 部分：

（因为 \(\boldsymbol{\omega} = \tilde{\boldsymbol{\omega}} - \boldsymbol{\varepsilon}_b\)，所以 \(\frac{\partial \boldsymbol{\omega}}{\partial \boldsymbol{\varepsilon}_b} = -\mathbf{I}\)）

零偏对自身：

零偏对四元数：

完整雅可比矩阵 \(F\)（\(7 \times 7\)）：

离散化（一阶）：

对应代码：QEAHRSUpdate L23-28 的 eye(7) + [...] *nts/2，其中矩阵就是 \(F\)（含 \(\frac{1}{2}\) 系数），乘 \(T_s\) 得 \(\Phi\)。

4. 量测模型——加计部分¶

加计测到的是重力在 b 系的投影。理论上，如果姿态完美：

即 \(f_x^b = -2(q_1 q_3 - q_0 q_2) \cdot g\)，\(f_y^b = -2(q_2 q_3 + q_0 q_1) \cdot g\)，\(f_z^b = -(q_0^2 - q_1^2 - q_2^2 + q_3^2) \cdot g\)

量测用水平分量 \([f_x^b, f_y^b]\)（对应 pitch 和 roll），去掉 \(f_z\)（因为 \(f_z\) 含 \(g\) 大小，提供不了额外信息）。

量测雅可比 \(H_{\text{acc}}\)（对四元数求偏导）：

展开后：

对应代码：QEAHRSUpdate L33-36。

5. 量测模型——磁力计部分¶

磁力计测到地磁场在 b 系的投影 \(\mathbf{m}^b\)。转到 n 系 \(\mathbf{m}^n = C_b^n \mathbf{m}^b\)，取水平分量算航向：

其中 \(C_b^n\) 的前两行前两列元素：

航向对四元数的偏导（链式法则）：

对应代码：QEAHRSUpdate L37-42。

6. 完整 EKF 流程¶

关键：QEKF 直接调用 PSINS 的通用 kfupdate.m 做预测+量测更新，自己只负责构建 \(\Phi\)、\(H\)、\(R\) 三个矩阵。这就是为什么先学 09_kfupdate.md 再看这里会非常轻松。

7. 自适应 R 矩阵¶

QEAHRSUpdate L48-53 根据水平加速度大小动态调整 \(R\)：

物理意义：水平加速度大 = 有运动 = 加计测的不只是重力 = 不信任加计 → 增大 \(R\) → KF 自动降低加计权重。

📝 逐行注释¶

QEAHRSInit.m 逐行注释¶

🧩 函数作用：初始化 7 状态 QEKF AHRS。

源码：QEAHRSInit.m

💡 P0/Q/R 的量级直觉： - \(P_0\) 大 = "我不确定" → KF 初始会快速修正 - \(Q\) 大 = "系统噪声大" → KF 持续保持修正能力 - \(R\) 大 = "量测噪声大" → KF 少信任量测 - 零偏 \(P_0 = 1000°/h\)：一开始完全不知道零偏多大 → 大初始不确定度 - 加计 \(R = 10\)mg：MEMS 加计噪声约几 mg → 10mg 是合理保守值

QEAHRSUpdate.m 逐行注释¶

🧩 函数作用：一步 QEKF AHRS 更新——构建 \(\Phi\)/\(H\)/\(R\) → 调 kfupdate → 归一化。

源码：QEAHRSUpdate.m

L54 是整个 QEKF 的灵魂：kfupdate(ahrs.kf, [0;0; yaw]) 做了两件事： 1. 预测步：用 L23-28 构建的 \(\Phi\) 推演状态和协方差 2. 量测步：用 L35-42 构建的 \(H\) 和 L50-52 构建的 \(R\) 做量测更新

QEKF 自己不做任何 KF 数学运算，只负责构建 \(\Phi\)/\(H\)/\(R\) 三个矩阵，然后交给通用 kfupdate。

L54 的量测 [0;0; yaw] 怎么理解？ 量测向量 \(\mathbf{z} = [z_1, z_2, z_3]^T\)： - \(z_1, z_2\)：加计水平分量的"测量值减预测值"的残差，这里写 0 是因为 \(H\) 矩阵已经把预测值编进去了（\(H \cdot \mathbf{x}\) 就是预测的加计值，量测值隐含在 acc 里） - \(z_3\)：磁航向角 yaw

严格来说 PSINS 的 kfupdate 内部会算 \(\mathbf{z} - H\mathbf{x}\)，所以这里传 [0;0;yaw] 实际上是 \(H\mathbf{x}\) 和 \(\mathbf{z}\) 的组合形式。如果看不懂没关系，记住"调 kfupdate 就完了"。

test_AHRS_QEKF.m 逐行注释¶

源码：test_AHRS_QEKF.m

和 test_AHRS_Mahony 的区别：QEKF 跑 100s（Mahony 跑 50s），因为 EKF 收敛需要更长的时间来估计零偏。磁力计=[0;0;0] 表示这个 test 不用磁力计，只靠加计做 pitch/roll。

📊 Mahony vs QEKF 对比¶

做航姿板建议：先上 Mahony 验证硬件 → 调好 PI 参数 → 再升级 QEKF 提升精度。Mahony 在 STM32H743 上跑 1kHz 毫无压力；QEKF \(7 \times 7\) 矩阵跑 1kHz 也没问题（H743 有双精度 FPU，\(7 \times 7\) 矩阵乘法约 343 次乘加，1kHz 下占 CPU <0.1%）。

🔍 断点调试建议¶

调试1：\(\Phi\) 矩阵验证¶

在 QEAHRSUpdate L23 设断点：

% 构建前
q0, q1, q2, q3  % 当前四元数
wx, wy, wz      % 当前角速度

% 手算 Φ 的 (1,2) 元素
phi12_man = -wx * nts / 2;
abs(ahrs.kf.Phikk_1(1,2) - (1 + 0) - phi12_man)  % =0 说明理解对了
% 注意：eye(7) 的 (1,2) = 0，加上 [...] * nts/2 的 (1,2) = -wx*nts/2

调试2：\(H\) 矩阵验证¶

在 QEAHRSUpdate L36 之后设断点：

% 加计部分 H (前2行)
ahrs.kf.Hk(1:2,:)

% 手算 h11, h12
h11_man = fx*q0 - fy*q3 + fz*q2;
h12_man = fx*q1 + fy*q2 + fz*q3;
% Hk(1,1) 应该 = h11*2, Hk(1,2) 应该 = h12*2
abs(ahrs.kf.Hk(1,1) - h11_man*2)  % <1e-15

调试3：协方差收敛验证¶

在 QEAHRSUpdate L54 设断点：

% KF 更新前
ahrs.kf.Pxk  % 先验协方差
ahrs.kf.Hk   % 量测矩阵
ahrs.kf.Rk   % 量测噪声（注意自适应 R）

% KF 更新后（dbstep 过 L54）
ahrs.kf.Kk   % 卡尔曼增益（应该随 P 减小而减小）
ahrs.kf.Pxk  % 后验协方差（应该比更新前小）

% 四元数归一化前后
norm(ahrs.kf.xk(1:4))  % 归一化前，应该接近 1 但不等于 1

调试4：Mahony vs QEKF 对比¶

用同一组 IMU 数据跑两个算法：

% 同一组数据，两个算法各跑一遍
ahrs1 = MahonyInit(4);
ahrs2 = QEAHRSInit(0.1);
% ... 循环更新 ...
att_mahony = m2att(ahrs1.Cnb);
att_qekf = m2att(ahrs2.Cnb);
norm(att_mahony - att_qekf)  % 稳态应该 <0.1°

❌ 初学者最容易踩的坑¶

1. 四元数没归一化：L55 qnormlz 是必须的！KF 更新后 \(\|\mathbf{q}\| \neq 1\)，不归一化 → DCB 误差 → 姿态发散
2. \(\Phi\) 矩阵的 \(\frac{1}{2}\) 系数忘了：L28 的 *nts/2 包含 \(\frac{1}{2}\)，因为 \(\dot{\mathbf{q}} = \frac{1}{2}\mathbf{q}\otimes\boldsymbol{\omega}\)。移植时容易写成 *nts 忘了除 2
3. \(H\) 矩阵的 \(\times 2\) 系数忘了：L36 的 *2 来自四元数→DCM 的 \(2\) 倍系数。DCM 元素含 \(q_i q_j \times 2\) 项，所以偏导也有 2
4. 自适应 R 方向搞反：L50 是动态时增大 \(R\)（不信任加计），不是减小。写反了会导致动态时加计噪声被放大
5. 磁力计补偿忘了去东向：Mahony 在 L39 做了 bxyz(1:2)=[0;norm(...)]，QEKF 在 L40 用 atan2(magH(1), magH(2)) 只取航向。不做这步 → 磁倾角会污染 roll/pitch
6. \(P_0\) 设太小：零偏初始 \(P_0 = 1000°/h\) 看着大，但如果你设成 \(1°/h\)（以为零偏很小），KF 会认为"零偏很确定"→ 不去估计零偏 → 零偏实际存在但没被估出来 → 姿态持续漂移

🎯 配套练习¶

练习1：\(\Phi\) 矩阵手算¶

在 QEAHRSUpdate L23 断点处，手算 \(\Phi\) 的第 1 行：

% 给定 q0=1, q1=0, q2=0, q3=0, wx=0.01, wy=0, wz=0
% Φ(1,:) = [1, -wx*Ts/2, -wy*Ts/2, -wz*Ts/2, q1*Ts/2, q2*Ts/2, q3*Ts/2]
phi1_man = [1, -wx*nts/2, 0, 0, 0, 0, 0];
norm(ahrs.kf.Phikk_1(1,:) - phi1_man)  % <1e-15

练习2：协方差收敛观察¶

跑 test_AHRS_QEKF.m，回答： - ahrs.kf.Pxk 对角线随时间怎么变？应该单调递减然后收敛 - 四元数的方差（Pxk(1:4)）和零偏方差（Pxk(5:7)）哪个收敛更快？为什么？ - ahrs.kf.Kk 随时间怎么变？应该随 \(P\) 减小而减小

练习3：自适应 R 观察¶

修改 test_AHRS_QEKF.m，在加计里加一个水平加速度（模拟动态）：

ahrs = QEAHRSUpdate(ahrs, [0,0,0], [0.5,0,1/glv.deg](<../../../0,0,0], [0.5,0,1/glv.deg>), [0;0;0], ts);
%                                                     ^^^ 改成 0.5 = 有水平加速度

练习4：Mahony vs QEKF 对比¶

用同一组数据（比如 test 里的静态 pitch=1° 场景）跑两个算法，对比： - 收敛时间：Mahony \(\approx \tau\)=4s，QEKF 收敛时间由 \(P_0/Q/R\) 决定 - 稳态精度：QEKF 应该略好（最优融合） - 零偏估计：QEKF 的 kf.xk(5:7) 是否收敛到 0？（因为模拟的陀螺没零偏）

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

把 QEAHRSUpdate.m 翻译成 C 伪代码，重点关注： - L23-28 的 \(\Phi\) 矩阵：\(7 \times 7\) 矩阵赋值，C 里怎么写？（提示：可以用 float Phi[7][7] 数组） - L54 的 kfupdate：需要哪些子函数？（提示：矩阵乘法、矩阵求逆、看 09_kfupdate.md 的 C 翻译） - \(7 \times 7\) 矩阵求逆：嵌入式上可以用简化方法吗？（提示：\(\Phi\) 大部分是 0，\(H\) 只有 3 行，可以分块运算）