第五章：Yaw 卡尔曼滤波增强
5.1 为什么 Mahony 的 Yaw 不准？
5.1.1 回顾 Mahony 的工作原理
在第四章中，我们学习了 Mahony 算法的核心思想：
加速度计测量的重力方向 ──┐
├──→ 叉积 ──→ 误差 ──→ PI控制器 ──→ 修正四元数
从四元数估计的重力方向 ──┘
关键点：Mahony 用加速度计作为"教官"，不断纠正陀螺仪的漂移。
5.1.2 问题来了：Yaw 没有"教官"
现在思考一个问题：加速度计能感知 Yaw 的变化吗？
做个实验：把 MPU6050 平放在桌上，然后水平旋转（像转盘一样）。
俯视图
旋转前 旋转后（Yaw 变了 90°）
↑ →
│ │
──────●────── ──────●──────
│ │
↓ ↓
重力方向：垂直向下 重力方向：还是垂直向下！
加速度计：az = 1g 加速度计：az = 1g（没变！）
结论：无论怎么水平旋转，重力方向始终是垂直向下的。加速度计感知不到 Yaw 的变化！
5.1.3 三个角的"待遇"不同
| 角度 | 旋转时重力方向变化？ | 加速度计能感知？ | Mahony 能修正？ |
|---|---|---|---|
| Roll | 变（重力在 Y-Z 平面分配） |  能 |
能 |
| Pitch | 变（重力在 X-Z 平面分配） | 能 |
能 |
| Yaw | 不变（重力始终沿 Z 轴） |  不能 |
不能 |
Mahony 的 Yaw 本质上就是纯陀螺仪积分，没有任何修正！
5.1.4 纯陀螺仪积分的问题：零偏漂移
陀螺仪有一个固有缺陷：零偏。
即使传感器完全静止，陀螺仪的读数也不是精确的 0，而是一个很小的值（比如 0.05°/s）。
理想情况：静止时 gz = 0
实际情况：静止时 gz = 0.05°/s（零偏）
Yaw 积分：yaw += gz × dt
= yaw + 0.05 × 0.01
= yaw + 0.0005°（每 10ms 漂移 0.0005°）
一分钟后：0.0005 × 100 × 60 = 3°
一小时后：3 × 60 = 180°（完全反向了！）
这就是为什么 Mahony 的 Yaw 会慢慢漂移。
5.1.5 实测现象
把 MPU6050 静止放置，观察 Mahony 输出的三个角度：
| 角度 | 现象 | 原因 |
|---|---|---|
| Roll | 稳定在 0° 附近 | 加速度计持续修正 |
| Pitch | 稳定在 0° 附近 | 加速度计持续修正 |
| Yaw | 缓慢漂移 | 无修正，零偏累积 |
5.1.6 怎么解决？
| 方案 | 原理 | 优缺点 |
|---|---|---|
| 加磁力计 | 用地磁场作为 Yaw 的绝对参考 | 最彻底，但需要额外硬件，受磁场干扰 |
| 卡尔曼滤波估计零偏 | 静止时学习零偏，运动时补偿 | 不需要额外硬件，但长时间仍会漂移 |
本章介绍第二种方案：用卡尔曼滤波估计陀螺仪零偏，改善 Yaw 漂移。
5.2 卡尔曼滤波基础
5.2.0 理解卡尔曼系统的 3 个核心问题
在学习公式之前，先问自己这 3 个问题：
| 核心问题 | 含义 | 例子（零偏估计） |
|---|---|---|
| 我想估计什么？ | 状态 x | 陀螺仪 Z 轴零偏 |
| 传感器能给我什么？ | 测量 z | 静止时的 gz 读数 |
| 状态怎么随时间变化？ | 预测模型 | 零偏基本不变 |
回答完这 3 个问题，剩下的就是套公式和调参。
调参：Q（状态变化多剧烈）、R（传感器多不准）
5.2.1 从一个生活问题开始
场景：你想知道自己的真实体重。
但你只有一个不太准的体重秤——每次称都显示不同的数字。
问题：怎么才能得到最准确的体重估计？
5.2.2 你有什么信息？
仔细想想，你其实有两个信息来源：
信息 1：今天的测量
早上称了一下，秤显示 70.5 kg。
但你知道这个秤有误差，可能偏高或偏低 0.5 kg。
| 概念 | 例子中 |
|---|---|
| 测量值 | 70.5 kg |
| 测量噪声 | ±0.5 kg（秤不准） |
信息 2：昨天的估计
昨天你估计自己是 70.0 kg，体重一天内不会变化太多。
| 概念 | 例子中 |
|---|---|
| 预测值 | 70.0 kg（昨天的估计） |
| 过程噪声 | ±0.1 kg（一天内的自然波动） |
5.2.3 核心问题：信谁？
现在你面临一个选择：
昨天估计：70.0 kg
今天测量：70.5 kg
真实体重到底是多少？
最朴素的想法：取平均？
(70.0 + 70.5) / 2 = 70.25 kg
但这合理吗？
- 如果秤很准（测量噪声小），应该更相信秤
- 如果昨天的估计很确信，应该更相信昨天
关键问题：怎么决定各自的权重？
5.2.4 关键洞察：量化"不确信程度"
要决定信谁，我们需要知道：对每个信息有多确信？
| 信息 | 不确信程度 | 符号 |
|---|---|---|
| 昨天的估计 | 对 70.0 kg 有多不确信？ | P |
| 今天的测量 | 对 70.5 kg 有多不确信？ | R |
直觉：
- 不确信程度大 → 不太可信 → 权重小
- 不确信程度小 → 比较可信 → 权重大
5.2.5 卡尔曼滤波的核心：自动计算最优权重
卡尔曼滤波的天才之处在于：它能自动算出最优的融合权重。
K = P / (P + R) ← 这就是"卡尔曼增益"
然后用这个权重融合两个信息：
最优估计 = 预测值 + K × (测量值 - 预测值)
= 70.0 + K × (70.5 - 70.0)
= 70.0 + K × 0.5
5.2.6 K 的行为分析
| 情况 | P 和 R 的关系 | K 的值 | 结果 |
|---|---|---|---|
| 预测不确信，测量准 | P 大，R 小 | K → 1 | ≈ 70.5（相信测量） |
| 预测确信，测量不准 | P 小，R 大 | K → 0 | ≈ 70.0（相信预测） |
| 两者差不多 | P ≈ R | K ≈ 0.5 | ≈ 70.25（各信一半） |
K 的本质：就是一个自适应的权重，由不确信程度自动决定。
5.2.7 卡尔曼滤波公式
上面用体重秤的例子建立了直觉，现在给出正式的数学公式。
符号说明
| 符号 | 含义 |
|---|---|
| $\hat{x}_k$ | 第 k 时刻的状态估计值 |
| $P_k$ | 第 k 时刻的估计协方差（不确定程度） |
| $z_k$ | 第 k 时刻的测量值 |
| $Q$ | 过程噪声（状态变化的不确定性） |
| $R$ | 测量噪声（传感器的不确定性） |
| $K_k$ | 第 k 时刻的卡尔曼增益 |
预测阶段
$$\hat{x}{k|k-1} = \hat{x}{k-1}$$
$$P_{k|k-1} = P_{k-1} + Q$$
- $\hat{x}_{k|k-1}$ 表示"用 k-1 时刻的信息预测 k 时刻的状态"
- 预测模型这里假设"状态不变"，实际可根据系统调整
更新阶段
$$K_k = \frac{P_{k|k-1}}{P_{k|k-1} + R}$$
$$\hat{x}k = \hat{x}{k|k-1} + K_k(z_k - \hat{x}_{k|k-1})$$
$$P_k = (1 - K_k) \cdot P_{k|k-1}$$
- $z_k - \hat{x}_{k|k-1}$ 称为新息（测量值与预测值的差）
- $K_k$ 决定新息对估计的影响程度
5.2.8 完整的卡尔曼滤波流程
卡尔曼滤波分为两个阶段，不断循环：
┌─────────────────────────────────────────────────────────────┐
│ │
│ 【预测阶段】根据上一时刻，预测当前状态 │
│ │
│ x_pred = x // 预测值（假设状态不变） │
│ P_pred = P + Q // 不确信程度增加 │
│ │
├─────────────────────────────────────────────────────────────┤
│ │
│ 【更新阶段】用测量值修正预测 │
│ │
│ K = P_pred / (P_pred + R) // 计算最优权重 │
│ x = x_pred + K × (z - x_pred) // 融合得到新估计 │
│ P = (1 - K) × P_pred // 更新不确信程度 │
│ │
└─────────────────────────────────────────────────────────────┘
│
↓
下一时刻继续...
5.2.9 预测阶段的直觉
x_pred = x // 体重不会突变，预测今天和昨天一样
P_pred = P + Q // 但时间越久，越不确信（也许今天吃多了？）
Q（过程噪声）的意义：状态本身会变化多少。
- Q 大 → 状态变化剧烈 → 预测很快变得不可信
- Q 小 → 状态基本不变 → 预测长期可信
P 和 Q 的本质区别（重要！）
初学者常见困惑：P 和 Q 有什么区别？为什么 K 的公式用 P 而不是 Q？
| Q（过程噪声） | P（协方差） | |
|---|---|---|
| 是什么 | 每一步增加多少不确定性 | 此刻总共有多少不确定性 |
| 变化吗 | 不变（固定参数） |
动态变化 |
| 类比 | 水龙头流速（固定） | 桶里水量（动态） |
P 受两个力作用：
Q 让 P 增大 测量让 P 减小
（越来越不确信） （获得信息，变确信）
+Q +Q +Q
P: ──→ ↗ ──→ ↘ ──→ ↗ ──→ ↘ ──→
预测 更新 预测 更新
-K·P -K·P
为什么 K 用 P 而不是 Q？
因为 K 回答的是"此刻该信谁"，需要的是当前的不确定程度（P），而不是每步的增量（Q）。
如果用 Q 代替 P：K = Q / (Q + R) 就变成固定值了，那和互补滤波没区别！
5.2.10 更新阶段的直觉
K = P_pred / (P_pred + R)
- 分子 P_pred：对预测的不确信程度
- 分母 P_pred + R：总的不确信程度
K 的范围是 0 ~ 1：
| K 值 | 含义 |
|---|---|
| K = 0 | 完全不信测量，只信预测 |
| K = 1 | 完全信测量，不信预测 |
| K = 0.3 | 信 30% 测量，70% 预测 |
x = x_pred + K × (z - x_pred)
z - x_pred：测量值和预测值的差距（称为"新息"）K × (z - x_pred)：根据差距调整多少
P = (1 - K) × P_pred
- 融合后，不确信程度降低了（因为获得了新信息）
- K 越大（越信测量），P 降低越多
5.2.11 数值例子
初始状态：
x = 70.0 kg（昨天估计）
P = 0.2 （对昨天估计的不确信程度）
Q = 0.01 （体重一天变化很小）
R = 0.5 （秤的测量噪声）
今天测量：z = 70.5 kg
预测阶段：
x_pred = 70.0
P_pred = 0.2 + 0.01 = 0.21
更新阶段：
K = 0.21 / (0.21 + 0.5) = 0.296
x = 70.0 + 0.296 × (70.5 - 70.0)
= 70.0 + 0.296 × 0.5
= 70.148 kg
P = (1 - 0.296) × 0.21 = 0.148
结果：估计体重从 70.0 更新为 70.148 kg，更接近测量值，但没有完全相信测量。
5.2.12 卡尔曼滤波 6 要素总结
从体重秤的例子中，我们自然地引出了卡尔曼滤波的全部要素：
| 要素 | 符号 | 体重例子 | 一句话解释 |
|---|---|---|---|
| 状态 | x | 真实体重 | 我们想估计的量 |
| 协方差 | P | 对体重估计的不确信程度 | 越大越不确信 |
| 过程噪声 | Q | 体重一天内的自然波动 | 状态会变化多少 |
| 测量值 | z | 秤的读数 | 传感器的输出 |
| 测量噪声 | R | 秤的误差 | 测量有多不准 |
| 卡尔曼增益 | K | 自动算出的权重 | 决定信谁多少 |
5.2.13 Q 和 R 的调参意义
| 参数 | 调大的效果 | 调小的效果 |
|---|---|---|
| Q | 预测快速变得不可信 → K 变大 → 更跟随测量 | 预测长期可信 → K 变小 → 更平滑 |
| R | 测量不可信 → K 变小 → 更平滑 | 测量可信 → K 变大 → 更跟随测量 |
调参口诀：
- 想要响应快：减小 R 或增大 Q
- 想要更平滑：增大 R 或减小 Q
- 本质上是调整 Q/R 的比值
5.2.14 与互补滤波的对比
| 对比项 | 互补滤波 | 卡尔曼滤波 |
|---|---|---|
| 权重 | 固定的 α（手动设定） | 自适应的 K（自动计算） |
| 不确定性 | 不考虑 | 核心要素 |
| 调参 | 调 α | 调 Q 和 R |
| 本质 | 固定权重的加权平均 | 最优权重的加权平均 |
一句话总结：卡尔曼滤波是"会自动调整权重的互补滤波"。
5.2.15 Q 和 R 的博弈：不是套娃，是闭环
初学者看公式时可能感觉"套娃"：K 影响 P，P 又影响 K，Q 和 R 到处都有…
其实不是套娃，而是按顺序执行的闭环。
每个周期就这 5 行代码
P = P + Q; // ① Q 让 P 变大
K = P / (P + R); // ② 用 P 和 R 算 K
x = x + K * (z - x); // ③ 用 K 更新状态
P = (1 - K) * P; // ④ 用 K 让 P 变小
// 下一周期回到 ①
闭环图示
┌─────────────────────────────────────────────┐
│ │
▼ │
[P] ──(+Q)──→ [P_pred] ──────┐ │
│ │
▼ │
K = P/(P+R) │
│ │
┌──────┴──────┐ │
▼ ▼ │
更新状态x P = (1-K)·P ───┘
│
▼
输出结果
Q 和 R 在"拔河"
| 参数 | 作用 | 最终效果 |
|---|---|---|
| Q 大 | P 涨得快 → K 变大 | 更信测量，响应快 |
| R 大 | K 直接变小 → P 降得慢 | 更信预测，更平滑 |
本质：Q 和 R 像两个调节旋钮，通过 P 和 K 互相博弈，最终自动找到平衡。
5.3 应用：用卡尔曼估计陀螺仪零偏
学完卡尔曼滤波的理论，现在把它应用到实际问题：估计陀螺仪 Z 轴的零偏，改善 Yaw 漂移。
5.3.1 回答 3 个核心问题
回顾 5.2.0 提出的框架，套用到零偏估计：
| 核心问题 | 答案 | 说明 |
|---|---|---|
| 我想估计什么？ | 陀螺仪 Z 轴零偏 $b$ | 静止时 gz 应该是 0，实际却是 $b$ |
| 传感器能给我什么？ | 静止时的 gz 读数 | 静止时 $z = b$（测量值就是零偏） |
| 状态怎么随时间变化？ | 零偏基本不变 | $b_{k} = b_{k-1}$（预测模型） |
5.3.2 为什么需要静止检测？
这是本方案的关键设计。
问题：运动时 gz ≠ 零偏
| 状态 | gz 读数 | 能否作为零偏测量？ |
|---|---|---|
| 静止 | gz = 零偏 | 可以 |
| 旋转 | gz = 零偏 + 真实角速度 | 不行 |
如果运动时也更新卡尔曼，会把真实角速度当成零偏，估计值会被污染。
解决方案：只在静止时更新
每个周期：
├── 检测是否静止
├── 如果静止 → 执行卡尔曼更新（学习零偏）
├── 如果运动 → 跳过卡尔曼更新（保持上次估计）
└── 用估计的零偏补偿 gz，然后积分得到 Yaw
静止检测条件
同时满足以下条件才认为静止：
| 条件 | 判断依据 | 阈值 |
|---|---|---|
| 加速度模值接近 1g | $0.95 < \sqrt{a_x^2+a_y^2+a_z^2} < 1.05$ | 排除加速运动 |
| 角速度都很小 | $|g_x|, |g_y|, |g_z| < 1.5°/s$ | 排除旋转运动 |
5.3.3 代码实现解析
参数定义
#define KALMAN_Q_BIAS 0.0001f /* 过程噪声：零偏变化多快 */
#define KALMAN_R_BIAS 0.01f /* 测量噪声：陀螺仪多不准 */
#define STATIC_ACCEL_MIN 0.95f /* 静止检测：加速度下限 */
#define STATIC_ACCEL_MAX 1.05f /* 静止检测：加速度上限 */
#define STATIC_GYRO_MAX 1.5f /* 静止检测：角速度阈值 */
状态变量
static float kal_bias = 0.0f; /* 状态：零偏估计值 */
static float kal_p_bias = 1.0f; /* 协方差：对估计的不确定程度 */
static float kal_yaw = 0.0f; /* Yaw 角积分结果 */
核心代码（逐行对应公式）
void kalman_yaw_update(mpu6050_t *data, float dt)
{
float gz = data->gz;
float accel_norm, gz_corrected;
float k, p_pred;
uint8_t is_static;
/* ========== 第一步：静止检测 ========== */
accel_norm = sqrtf(data->ax*data->ax + data->ay*data->ay + data->az*data->az);
gz_corrected = gz - kal_bias; /* 用当前估计修正 */
is_static = (accel_norm > STATIC_ACCEL_MIN &&
accel_norm < STATIC_ACCEL_MAX &&
fabsf(data->gx) < STATIC_GYRO_MAX &&
fabsf(data->gy) < STATIC_GYRO_MAX &&
fabsf(gz_corrected) < STATIC_GYRO_MAX);
/* ========== 第二步：卡尔曼更新（仅静止时）========== */
if (is_static)
{
/* 预测阶段 */
// x_pred = x; （零偏不变，省略）
p_pred = kal_p_bias + KALMAN_Q_BIAS; // P = P + Q
/* 更新阶段 */
k = p_pred / (p_pred + KALMAN_R_BIAS); // K = P / (P + R)
kal_bias = kal_bias + k * (gz - kal_bias); // x = x + K(z - x)
kal_p_bias = (1.0f - k) * p_pred; // P = (1 - K) * P
}
/* ========== 第三步：Yaw 积分 ========== */
kal_yaw += gz_corrected * dt;
/* 角度归一化到 -180 ~ 180 */
while (kal_yaw > 180.0f) kal_yaw -= 360.0f;
while (kal_yaw < -180.0f) kal_yaw += 360.0f;
data->yaw = kal_yaw;
}
代码与公式对照
| 代码 | 对应公式 | 含义 |
|---|---|---|
p_pred = kal_p_bias + Q |
$P_{k | k-1} = P_{k-1} + Q$ |
k = p_pred / (p_pred + R) |
$K = \frac{P}{P+R}$ | 计算卡尔曼增益 |
kal_bias = kal_bias + k*(gz - kal_bias) |
$\hat{x} = \hat{x} + K(z - \hat{x})$ | 融合测量更新估计 |
kal_p_bias = (1-k) * p_pred |
$P = (1-K) \cdot P$ | 更新不确定程度 |
5.3.4 参数选择与调参
Q 和 R 的物理意义
| 参数 | 值 | 物理意义 |
|---|---|---|
| Q = 0.0001 | 很小 | 零偏变化很慢（温漂是缓慢的） |
| R = 0.01 | 较小 | 陀螺仪噪声不大（静止时读数比较稳定） |
Q/R 比值的影响
$$\frac{Q}{R} = \frac{0.0001}{0.01} = 0.01$$
比值很小，说明：
- 系统更相信预测（零偏不会突变）
- 卡尔曼增益 K 会比较小
- 零偏估计变化平滑缓慢
调参建议
| 现象 | 调整方向 |
|---|---|
| 零偏估计跟踪太慢 | 增大 Q 或减小 R |
| 零偏估计抖动太大 | 减小 Q 或增大 R |
| 静止时 Yaw 仍漂移 | 检查静止检测阈值是否合理 |
5.3.5 算法流程图
┌─────────────────────────────────────────────────────────────┐
│ 每个采样周期 │
├─────────────────────────────────────────────────────────────┤
│ │
│ ┌─────────────┐ │
│ │ 读取传感器 │ ax, ay, az, gx, gy, gz │
│ └──────┬──────┘ │
│ ↓ │
│ ┌─────────────┐ │
│ │ 静止检测 │ 加速度≈1g 且 角速度≈0？ │
│ └──────┬──────┘ │
│ ↓ │
│ ┌────┴────┐ │
│ ↓ ↓ │
│ [静止] [运动] │
│ ↓ ↓ │
│ 卡尔曼 跳过 │
│ 更新零偏 (保持上次) │
│ ↓ ↓ │
│ └────┬────┘ │
│ ↓ │
│ ┌─────────────┐ │
│ │ gz - 零偏 │ 补偿后的角速度 │
│ └──────┬──────┘ │
│ ↓ │
│ ┌─────────────┐ │
│ │ Yaw += ω*dt │ 积分得到 Yaw 角 │
│ └─────────────┘ │
│ │
└─────────────────────────────────────────────────────────────┘
5.3.6 小结
| 要点 | 说明 |
|---|---|
| 核心思想 | 静止时学习零偏，运动时用零偏补偿 |
| 为什么有效 | 静止时 gz = 零偏，是完美的测量机会 |
| 局限性 | 长时间不静止，零偏估计无法更新 |
| 适用场景 | 间歇运动的应用（会有静止时刻） |
5.3.7 实际效果
这里我们用匿名上位机来观测，第一个roll我们改成偏移，第二个是hwt101的yaw,第三个是我们的yaw
ano_send_euler(&huart1, kalman_get_bias(), HWT101_GetYaw(&hwt101), mpu_data.yaw);
5.3.8 方法的缺陷
虽然卡尔曼零偏估计能改善 Yaw 漂移，但它不是完美的解决方案。
缺陷一：依赖静止时刻
| 场景 | 问题 |
|---|---|
| 长时间持续运动 | 无法更新零偏，估计值可能过时 |
| 无人机飞行 | 飞行过程中几乎不会静止 |
| 车辆行驶 | 只有停车时才能更新 |
后果：零偏会随温度缓慢变化（温漂），如果长时间不静止，估计值会逐渐偏离真实值。
缺陷二：静止检测可能误判
| 误判类型 | 原因 | 后果 |
|---|---|---|
| 假阳性（运动误判为静止） | 阈值设置过宽松 | 真实角速度被当成零偏，估计被污染 |
| 假阴性（静止误判为运动） | 阈值设置过严格 | 错过学习机会，零偏更新变慢 |
缺陷三：无绝对参考
与 Roll/Pitch 不同，Yaw 没有绝对参考：
| 角度 | 绝对参考 | 能否完全修正漂移？ |
|---|---|---|
| Roll | 重力方向 | 能（加速度计） |
| Pitch | 重力方向 | 能（加速度计） |
| Yaw | 无 | 只能减缓，无法消除 |
结论：卡尔曼零偏估计只能减缓 Yaw 漂移，无法消除。长时间运行后仍会累积误差。
缺陷四：初始收敛需要时间
系统启动时，零偏估计从初始值（通常是 0）开始收敛到真实值，需要一定时间：
启动 → 静止等待 → 零偏逐渐收敛 → 估计可用
如果启动后立即运动，零偏估计可能不准。
彻底解决 Yaw 漂移的方案
| 方案 | 原理 | 优缺点 |
|---|---|---|
| 加磁力计 | 用地磁场作为 Yaw 的绝对参考 | 彻底解决漂移 受磁场干扰（电机、铁器） |
| 加 GPS | 用航向角作为参考 | 不受磁场干扰 需要运动才有航向，室内不可用 |
| 视觉定位 | 用摄像头识别环境特征 | 高精度 计算量大，成本高 |
本方案的定位：在无额外传感器的条件下，尽可能改善 Yaw 漂移。是一种低成本的折中方案。
5.4 本章总结
问题回顾
Mahony 算法用加速度计修正陀螺仪漂移，但加速度计感知不到 Yaw 旋转（水平转动时重力方向不变），导致 Yaw 本质上是纯陀螺仪积分，会因零偏而漂移。
解决思路
问题：Yaw 没有"教官"（绝对参考）
↓
洞察：静止时，陀螺仪读数 = 零偏（这就是测量机会）
↓
方案：用卡尔曼滤波估计零偏，运动时补偿
卡尔曼滤波核心认知
| 要点 | 说明 |
|---|---|
| 本质 | 两个不完美信息的最优融合 |
| 3个核心问题 | 估计什么？测量什么？怎么预测？ |
| P vs Q | P 是动态的不确定程度，Q 是每步增量 |
| 公式 | 预测 → 计算K → 更新状态 → 更新P |
零偏估计方案要点
| 要点 | 内容 |
|---|---|
| 状态 | 陀螺仪 Z 轴零偏 |
| 测量 | 静止时的 gz 读数 |
| 关键设计 | 静止检测——只在静止时更新 |
| 效果 | 静止时 Yaw 不漂移 |
| 局限 | 长时间运动无法更新，只能减缓不能消除 |
各章算法对比
| 章节 | 算法 | Roll/Pitch | Yaw | 复杂度 |
|---|---|---|---|---|
| 第二章 | 纯加速度计 | 静止准，动态抖 | 无法解算 |
 |
| 第三章 | 互补滤波 | 融合 |
无法解算 |
|
| 第四章 | Mahony | 四元数融合 |
 纯积分，漂移 |
|
| 第五章 | Mahony + 卡尔曼 | 四元数融合 |
零偏补偿，减缓漂移 |
|
如果还想更好？
| 方向 | 方案 |
|---|---|
| 彻底消除 Yaw 漂移 | 加磁力计（9轴融合） |
| 更高精度 | 扩展卡尔曼滤波（EKF） |
| 工业级方案 | IMU + GPS + 视觉融合 |