一阶互补滤波（First-Order Complementary Filter）分析¶

对标 Mahony 滤波的学习笔记，从最简单的一阶线性互补滤波讲起，逐层深入。 本文分析蓝本：MPU6050_light 开源库（Arduino 生态广泛使用的轻量级 MPU6050 驱动） GitHub 仓库：https://github.com/rfetick/MPU6050_light

先明确坐标系约定（和 Mahony 一致）¶

在牛小骥老师团队的 NED+FRD 约定下：

重力方向：导航系中重力沿 +Z 方向（指向地），即 \(\mathbf{g}^n = [0, 0, g]^T\)

注意：一阶互补滤波工作在欧拉角空间，不涉及四元数，适合小角度场景。

第一部分：定义和初始化¶

1.1 宏定义与参数¶

以 MPU6050_light 库为例（MPU6050_light.h 第 29-34 行）：

#define RAD_2_DEG             57.29578 // [deg/rad]
#define CALIB_OFFSET_NB_MES   500      // 校准时采样次数
#define DEFAULT_GYRO_COEFF    0.98     // 陀螺仪权重 (互补滤波系数)

参数说明：

补充：α 的物理意义

\(\alpha = 0.98\) 意味着： - 98% 的信息来自陀螺仪（高频动态好） - 2% 的信息来自加速度计（低频绝对参考）

对应的截止频率约为：

\[ f_c = \frac{1-\alpha}{2\pi \cdot \alpha \cdot \Delta t} \]

代入 \(\alpha=0.98, \Delta t=0.01\)s（典型 100Hz 采样）：

\[ f_c = \frac{0.02}{2\pi \times 0.98 \times 0.01} \approx 0.32 \text{ Hz} \]

意思是：低于 0.32 Hz 的缓慢姿态变化，主要由加速度计决定；高于 0.32 Hz 的快速运动，主要由陀螺仪跟踪。

1.2 类成员变量（滤波器状态）¶

（MPU6050_light.h 第 104-110 行）

float gyroXoffset, gyroYoffset, gyroZoffset;  // 陀螺仪零偏
float accXoffset, accYoffset, accZoffset;     // 加速度计零偏

float angleAccX, angleAccY;   // 加速度计计算的角度
float angleX, angleY, angleZ; // 互补滤波输出的角度
float filterGyroCoef;         // 互补滤波系数 (陀螺仪权重)

变量分类：

💡 一阶滤波为什么没有积分状态变量？

因为一阶互补滤波的零偏是开机校准一次就固定的，不需要在线更新。 只有二阶/PI版本的互补滤波才会有积分状态变量（在线估计零偏）。 这是一阶和二阶很大的区别！

第二部分：陀螺仪零偏校准¶

2.1 校准代码¶

这是一阶互补滤波的标准前置步骤（MPU6050_light.cpp 第 138-166 行）：

void MPU6050::calcOffsets(bool is_calc_gyro, bool is_calc_acc){
  if(is_calc_gyro){ setGyroOffsets(0,0,0); }
  if(is_calc_acc){ setAccOffsets(0,0,0); }
  float ag[6] = {0,0,0,0,0,0}; // 3*acc, 3*gyro

  for(int i = 0; i < CALIB_OFFSET_NB_MES; i++){
    this->fetchData();
    ag[0] += accX;
    ag[1] += accY;
    ag[2] += (accZ-1.0);    // 加速度计Z轴静止时应为1g
    ag[3] += gyroX;
    ag[4] += gyroY;
    ag[5] += gyroZ;
    delay(1);               // 两次测量间稍作等待
  }

  if(is_calc_acc){
    accXoffset = ag[0] / CALIB_OFFSET_NB_MES;
    accYoffset = ag[1] / CALIB_OFFSET_NB_MES;
    accZoffset = ag[2] / CALIB_OFFSET_NB_MES;
  }

  if(is_calc_gyro){
    gyroXoffset = ag[3] / CALIB_OFFSET_NB_MES;
    gyroYoffset = ag[4] / CALIB_OFFSET_NB_MES;
    gyroZoffset = ag[5] / CALIB_OFFSET_NB_MES;
  }
}

2.2 数学原理¶

陀螺仪的测量模型：

其中： - \(\omega_{\text{true}}\) — 真实角速度 - \(b\) — 零偏（bias，恒定或缓变） - \(n\) — 噪声（高频随机噪声）

静止时 \(\omega_{\text{true}} = 0\)，多次采样取平均后噪声被抵消：

为什么要校准零偏？

假设陀螺零偏是 0.5°/s（很常见的精度），积分 1 分钟后：

\[ 0.5°/s \times 60s = 30° \]

仅仅 1 分钟，纯积分就会漂走 30 度了！这就是为什么必须校准零偏。

但即使校准了，零偏还会随温度漂移——这就是一阶滤波的局限性，也是为什么需要二阶/PI版本（在线估计零偏）。

第三部分：加速度计求绝对角度¶

3.1 代码实现¶

核心是两个 atan2 计算（MPU6050_light.cpp 第 196-198 行）：

float sgZ = accZ<0 ? -1 : 1; // 处理翻转情况，支持-180到+180度
angleAccX =   atan2(accY, sgZ*sqrt(accZ*accZ + accX*accX)) * RAD_2_DEG;
angleAccY = - atan2(accX,     sqrt(accZ*accZ + accY*accY)) * RAD_2_DEG;

RAD_2_DEG = 180 / π，弧度转角度的系数。

3.2 数学推导（为什么是 atan2？）¶

基本思想：静止时加速度计只感受重力，重力方向就是竖直向下的参考方向。

在载体系中测量到的加速度：

展开方向余弦矩阵：

简化（只有Z列有效）：

Roll 角推导： 用 \(a_y\) 除以 \(a_z\)：

title:📐 **数学严谨性说明**
collapse:close
上式成立的前提是 $\cos\theta \neq 0$，即俯仰角 $\theta \neq \pm 90^\circ$（万向节锁奇异位置）。
在小角度假设下（$|\theta| < 60^\circ$），$\cos\theta > 0.5$，近似效果良好；
接近 90° 时该式会迅速发散。这也是高阶算法必须切换到四元数/旋转矩阵的原因之一。

所以：

Pitch 角推导： 用 \(a_x\) 除以 \(\sqrt{a_y^2 + a_z^2}\)：

所以：

title: 补充 从导航系到载体系的预测重力
icon: fire

上式 $\mathbf{a}^b = \mathbf{C}_n^b \cdot \mathbf{g}^n = \mathbf{C}_n^b \cdot \begin{bmatrix} 0 \\ 0 \\ g \end{bmatrix}$ 中，按道理应该是 $\hat{\mathbf{g}}^b = \mathbf{C}_n^b \cdot \mathbf{g}^n$


$\hat{\mathbf{g}}^b$是假设加速度计只感受重力，然后从n系转到b系的预测重力（估计值）
加速度计测量值一般记为 $\widetilde{a}^b$ 或 $a_{meas}^b$​。


| 符号                                          | 含义                                 | 来源    | 是否估计值 |
| ------------------------------------------- | ---------------------------------- | ----- | ----- |
| $\mathbf{g}^n$                              | 导航系中的重力向量（已知）                      | 定义    | 否（真值） |
| $\mathbf{C}_n^b$ 或 $\mathbf{q}_b^n$         | 当前估计的姿态                            | 滤波器状态 | 是（估计） |
| $\hat{\mathbf{g}}^b$                        | 根据姿态将 $\mathbf{g}^n$ 变换到载体系得到的预测重力 | 计算    | **是** |
| $\mathbf{a}^b$ 或 $\mathbf{a}_{\text{meas}}$ | 加速度计实际测量值                          | 传感器   | 否（观测） |

这里写的是 $\mathbf{a}^b$ 是为了和代码对应，请读者务必搞清楚这一点！

3.3 直观理解¶

想象手里拿着一块IMU板：

⚠️ 重要前提：加速度计只有在静止或匀速运动时才能正确测量角度。

如果飞机/机器人在加速，加速度计测量的是"重力 + 运动加速度"，算出来的角度就不准了（MEMS 加计的输出噪声太大）。 这就是为什么需要互补滤波——快速运动时主要看陀螺仪。

🛠️ 工程进阶：载体动态干扰的定量分析与对策

数学模型：加速度计测量值 = 比力（重力投影）+ 运动加速度

\[ \mathbf{a}_{\text{meas}} = \mathbf{C}_n^b \mathbf{g}^n + \mathbf{a}_{\text{dynamic}} \]

静态时 \(\mathbf{a}_{\text{dynamic}} \approx 0\)，模长 \(\|\mathbf{a}_{\text{meas}}\| \approx 1g\)； 动态时模长会偏离 1g，偏离程度反映了干扰大小。

工程对策：动态权重自适应

检测加速度模长与 1g 的偏差：

\[ \Delta = \left| \|\mathbf{a}^b\|_2 - 1g \right| \]

当 \(\Delta > \text{threshold}\)（典型阈值 0.1g~0.3g）时，说明动态干扰过大， 应动态调小加速度计权重 \((1-\alpha)\)，极端情况下完全信任陀螺仪（\(\alpha \to 1\)）：

\[ \alpha_{\text{dyn}} = 1 - (1-\alpha) \cdot \max\left(0, 1 - \frac{\Delta}{\Delta_{\max}}\right) \]

这样可以防止滤波器被剧烈运动产生的错误加速度数据"带偏"。

第四部分：一阶互补滤波核心公式¶

4.1 代码（最关键的一行！）¶

（MPU6050_light.cpp 第 206-208 行）

// 一阶互补滤波：X轴（Roll）
angleX = wrap(filterGyroCoef * (angleAccX + wrap(angleX + gyroX*dt - angleAccX, 180)) 
            + (1.0 - filterGyroCoef) * angleAccX, 180);

// 一阶互补滤波：Y轴（Pitch）
angleY = wrap(filterGyroCoef * (angleAccY + wrap(angleY + sgZ*gyroY*dt - angleAccY, 90)) 
            + (1.0 - filterGyroCoef) * angleAccY, 90);

// Z轴（Yaw）：纯积分，无修正
angleZ += gyroZ * dt; // not wrapped

角度环绕处理（wrap）：wrap() 函数用于处理角度环绕（例如从 +179° 转到 -179° 时不会算成转了一整圈）。 去掉 wrap 之后，核心公式非常简洁：

angleX = alpha * (angleX + gyroX * dt) + (1 - alpha) * angleAccX;

就这么简单！一行代码就是一个滤波器。

sgZ 的作用：在 angleAccY 公式中出现 sgZ*gyroY*dt，这个 sgZ 由 accZ 的符号决定。它的本质是处理 IMU 倒置安装的情况（Z 轴向上）。在标准 NED 系（Z 向下）中 sgZ = 1，不影响公式。

4.2 数学公式¶

其中： - \(\theta_k\) — 当前时刻的估计角度（输出） - \(\theta_{k-1}\) — 上一时刻的估计角度（状态） - \(\omega_k\) — 当前陀螺仪角速度（已扣零偏） - \(\Delta t\) — 采样周期 - \(\theta_{\text{acc}}\) — 加速度计计算的绝对角度 - \(\alpha\) — 互补滤波系数（0.95~0.99）

4.3 三种理解角度（层层深入）¶

角度 1：加权平均（最直观）¶

其中 \(\theta_{\text{gyro}} = \theta_{k-1} + \omega_k \Delta t\)

就是"我信陀螺98%，信加速度计2%"的直白表达。

角度 2：反馈修正（控制论视角）¶

把公式变形一下：

即：

其中： - \(\theta_{\text{pred}} = \theta_{k-1} + \omega_k \Delta t\) — 陀螺仪积分预测 - \(e = \theta_{\text{acc}} - \theta_{\text{pred}}\) — 预测误差 - \(K_p = 1-\alpha\) — 比例增益

🎯 这就是一个比例控制器（P控制器）！

控制系统框图解读： - 前向通道：陀螺仪积分器（从角速度到角度） - 反馈通道：加速度计（提供绝对角度参考） - 比较环节：误差 = 参考值 - 预测值 - 控制器：比例增益 \(K_p\)

和卡尔曼滤波的"预测-更新"框架完全一致，只是卡尔曼的增益是动态计算的，这里是固定的。 可以建议理解：一阶互补滤波 = 固定增益的稳态卡尔曼滤波（Steady-State Kalman Filter）

角度 3：频域互补（最本质）¶

对公式做Z变换，可以得到从加速度计到输出的传递函数：

这是一个一阶低通滤波器（Low-Pass Filter）。

同样，从陀螺仪角速度到输出角度的传递函数：

包含一个积分器和一个高通特性。

为什么叫"互补"？

低通 + 高通 = 全通（两者相加等于1）

• 低频段：加速度计说了算（提供绝对参考）
• 高频段：陀螺仪说了算（提供动态响应）
• 两者完美互补，覆盖全频段

第五部分：Yaw 角怎么办？¶

5.1 代码（纯积分，无修正）¶

// Yaw 角：只有陀螺仪积分，没有加速度计修正
angleZ += gyroZ * dt;  // not wrapped

5.2 为什么加速度计不能修正 Yaw？¶

因为重力方向是沿 Z 轴的，绕 Z 轴旋转（Yaw）时，重力在三个轴上的分量完全不变。

数学上看：

Yaw 角 \(\psi\) 只出现在矩阵的前两列，但重力只有第三列有值，所以 \(\psi\) 的变化不会影响加速度计的读数。

结论：纯IMU（加速度计+陀螺仪）永远测不出绝对Yaw角，Yaw一定会漂移。

要得到绝对Yaw，必须增加： - 🧭 磁力计（电子罗盘） - 🛰️ GPS（航向角） - 📷 视觉（视觉SLAM）

5.3 加入磁力计的一阶互补（扩展）¶

如果有磁力计，可以对Yaw也做互补滤波：

// 用磁力计计算航向角（需要先做倾斜补偿，这里简化）
float yaw_mag = calculate_yaw_from_mag(mx, my, mz, roll, pitch);

// Yaw 也做一阶互补
yaw_angle = alpha * (yaw_angle + gyro_z * dt) + (1.0f - alpha) * yaw_mag;

注意：磁力计容易受环境磁场干扰（电机、钢筋、磁铁等），实际使用中权重通常比加速度计更低（如 0.995 甚至 0.999）。

第六部分：完整函数逐行解析¶

6.1 函数签名¶

（MPU6050_light.cpp 第 191 行）

void MPU6050::update();

说明： - 无参数，无返回值 - 内部读取传感器数据 → 计算加速度角度 → 互补滤波融合 → 更新状态 - 需要在 loop 中尽可能频繁调用

6.2 步骤 1：读取原始数据并扣除零偏¶

（MPU6050_light.cpp 第 169-189 行）

void MPU6050::fetchData(){
  // 通过 I2C 读取 14 字节数据（加速度+温度+陀螺仪）
  wire->beginTransmission(address);
  wire->write(MPU6050_ACCEL_OUT_REGISTER);
  wire->endTransmission(false);
  wire->requestFrom(address,(uint8_t) 14);

  int16_t rawData[7]; // [ax,ay,az,temp,gx,gy,gz]
  for(int i=0;i<7;i++){
    rawData[i]  = wire->read() << 8;
    rawData[i] |= wire->read();
  }

  // 转换单位 + 扣除零偏
  accX = ((float)rawData[0]) / acc_lsb_to_g - accXoffset;
  accY = ((float)rawData[1]) / acc_lsb_to_g - accYoffset;
  accZ = (!upsideDownMounting - upsideDownMounting) * ((float)rawData[2]) / acc_lsb_to_g - accZoffset;
  temp = (rawData[3] + TEMP_LSB_OFFSET) / TEMP_LSB_2_DEGREE;
  gyroX = ((float)rawData[4]) / gyro_lsb_to_degsec - gyroXoffset;
  gyroY = ((float)rawData[5]) / gyro_lsb_to_degsec - gyroYoffset;
  gyroZ = ((float)rawData[6]) / gyro_lsb_to_degsec - gyroZoffset;
}

对应公式：

零偏是开机时一次性校准好的，运行中不再改变。 这是一阶滤波的典型特征——没有积分环节在线修正零偏。

6.3 步骤 2：加速度计算绝对角度¶

（MPU6050_light.cpp 第 196-198 行）

float sgZ = accZ<0 ? -1 : 1; // 支持-180°到+180°范围
angleAccX =   atan2(accY, sgZ*sqrt(accZ*accZ + accX*accX)) * RAD_2_DEG;
angleAccY = - atan2(accX,     sqrt(accZ*accZ + accY*accY)) * RAD_2_DEG;

对应公式：

这里 sgZ 是一个符号位，用来处理 IMU 倒置（Z轴向上）的情况，让 Roll 角可以覆盖 -180° 到 +180° 的完整范围。 如果是正常安装（Z轴向下，NED系），sgZ = 1，公式就退化为标准形式。

6.4 步骤 3：计算时间差 dt¶

（MPU6050_light.cpp 第 200-202 行）

unsigned long Tnew = millis();
float dt = (Tnew - preInterval) * 1e-3;
preInterval = Tnew;

作用： - 用系统毫秒计数器计算两次采样的时间间隔 - 因为 loop() 的执行周期不固定，所以必须实测 dt - * 1e-3 是把毫秒转换成秒

⚠️ 工程小问题：millis() 的时间量化抖动 millis() 的分辨率只有 1 ms。对于常规 100Hz 采样（\(\Delta t = 10\text{ ms}\)）， 算出来的 dt 会在 9 ms、10 ms、11 ms 之间频繁跳动，带来高达 10% 的量化噪声， 直接污染陀螺仪的积分环节。 改进：高性能嵌入式场景强烈建议改用 micros() 获取微秒级时间戳， 确保 \(\Delta t\) 的时间确定性，姿态解算精度可提升至少一个数量级。注意系数从 1e-3 变为 1e-6f（加上 f 显式指定单精度浮点数，避免 MCU 进行双精度隐式转换消耗算力）。 如果系统非常简单（比如只是个单纯的姿态测量模块，没有复杂的通信和外设干扰）：使用裸机 + micros() 算 dt 就足够精准了，引入 FreeRTOS 反而增加了任务切换的系统开销。若系统需要执行多任务切换等等，采用 freertos 可以减少开发和维护成本。

6.5 步骤 4：互补滤波融合（核心！）¶

（MPU6050_light.cpp 第 206-208 行）

// Roll 轴互补滤波（带角度环绕处理）
angleX = wrap(filterGyroCoef * (angleAccX + wrap(angleX + gyroX*dt - angleAccX, 180)) 
            + (1.0 - filterGyroCoef) * angleAccX, 180);

// Pitch 轴互补滤波（带角度环绕处理）
angleY = wrap(filterGyroCoef * (angleAccY + wrap(angleY + sgZ*gyroY*dt - angleAccY, 90)) 
            + (1.0 - filterGyroCoef) * angleAccY, 90);

// Yaw 轴：纯积分
angleZ += gyroZ * dt; // not wrapped

对应公式：

三个轴分别计算，互不影响——这是欧拉角方法的特点（也是万向节锁的来源）。

wrap() 函数处理角度跳变：比如角度从 +179° 增加 2°，按正常算数会得到 181°，但 wrap 后变成 -179°，符合物理直觉。

⚠️ 工程深度：欧拉角空间的"轴向耦合"缺陷 一阶互补滤波将 Roll 和 Pitch 拆成两个独立的 1D 滤波器处理，这在小角度单轴运动时完全成立。 但在大角度多轴同时翻转（如 Roll=45° 且 Pitch=45°）时，由于旋转的非对易性， 角速度在空间上存在几何耦合——横滚角速度会分流到俯仰轴上，反之亦然。 数学本质：欧拉角微分方程中存在耦合项

\[ \begin{bmatrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{bmatrix} = \begin{bmatrix} 1 & \sin\phi\tan\theta & \cos\phi\tan\theta \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi/\cos\theta & \cos\phi/\cos\theta \end{bmatrix} \begin{bmatrix} p \\ q \\ r \end{bmatrix} \]

当 \(\theta \to 0\)（小角度）时，矩阵退化为单位阵，三轴解耦； 当 \(\theta\) 增大时，非对角项逐渐显著，独立 1D 积分会产生几何变形。 这也是 Mahony 为啥要切换到四元数/旋转矩阵空间进行三轴整体纠偏的原因了吧。

6.6 wrap 辅助函数¶

（MPU6050_light.cpp 第 13-17 行）

static float wrap(float angle, float limit){
  while (angle >  limit) angle -= 2*limit;
  while (angle < -limit) angle += 2*limit;
  return angle;
}

作用：把角度限制在 [-limit, +limit] 范围内，处理周期性环绕。

第七部分：一阶 → 二阶 → 非线性（演化路径）¶

理解了一阶，就能顺理成章地理解更高阶的滤波。这里和你已经掌握的 Mahony 做个对比。

7.1 演化关系图¶

一阶互补滤波
  │
  ├─ 特点：角度空间 + 固定零偏 + P控制
  └─ 问题：零偏随温度漂移 → 加积分环节
       │
       ▼
  二阶互补滤波（带PI）
     ├─ 特点：角度空间 + 在线估计零偏 + PI控制
     └─ 问题：大角度下欧拉角非线性 + 万向节锁 → 换四元数+叉积
          │
          ▼
  Mahony 滤波（非线性互补滤波）
     ├─ 特点：四元数 + 叉积误差 + PI控制
     └─ 问题：固定增益，不能应对动态噪声 → 卡尔曼滤波
          │
          ▼
  ESKF / EKF
     └─ 特点：误差状态卡尔曼滤波，增益随噪声动态调整

7.2 一阶 vs 二阶 vs Mahony 对比表¶

7.3 从一阶到二阶：只加三行代码¶

// 计算误差
float error_roll = roll_acc - roll_angle;

// 积分项（在线估计零偏）— 这是二阶比一阶多出来的
gyro_x_offset += Ki * error_roll * dt;  

// 还是原来的互补公式，但零偏会自己变
roll_angle = alpha * (roll_angle + (gx - gyro_x_offset) * dt) 
           + (1 - alpha) * roll_acc;

我理解的：核心区别就是多了一个积分项来在线更新零偏。

一阶：零偏固定 → 会随温度漂移 二阶：零偏随误差积分 → 自动跟踪零偏变化

再下一步：把"角度相减"换成"向量叉积"，把"欧拉角积分"换成"四元数更新"——就是 Mahony 了。

代码与数学符号对照表¶

总结：一阶互补滤波的核心思想¶

1. 陀螺仪积分：给出快速、平滑的动态姿态
2. 加速度计修正：提供绝对参考，消除漂移
3. 加权融合：按频率分配权重，高低频互补
4. 零偏校准：开机静止采样，扣除陀螺零偏

一句话概括：

"陀螺仪管快跑（高频），加速度计管方向（低频），两者一互补，既快又准。"

参考资料¶

开源代码¶

1. MPU6050_light（本文分析蓝本）：
2. 仓库：https://github.com/rfetick/MPU6050_light

3. 特点：Arduino 生态广泛使用，代码规范，注释清晰

4. 平衡车经典实现（中文注释，入门友好）：

5. 仓库：https://github.com/fomalhaut251/Balance-car

6. 特点：包含一阶和二阶互补滤波，配CSDN博客详解

7. Mahony 滤波（进阶学习）：

8. 仓库：https://github.com/wild-civil/STM32_BMI088_MahonyAHRS
9. 特点：STM32 平台，和本文分析结构一致

理论资料¶

1. 互补滤波原理讲解：

2. https://ahrs.readthedocs.io/en/latest/filters/complementary.html

3. 四元数与姿态解算基础：

4. https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation

5. 进阶：从互补滤波到卡尔曼滤波：

6. 可以理解为"卡尔曼滤波收敛到稳态时，就是互补滤波"

title: ## 💡 进阶思考与工程坑点（备忘）
collapse: close
### 1. 源码中的时间量化噪声（小小细节）
* **现象**：源码中使用 `millis()` 计算 `dt`：`float dt = (Tnew - preInterval) * 1e-3;`
* **坑点**：`millis()` 的分辨率只有 1ms。在 100Hz（10ms 周期）采样时，`dt` 会在 9ms、10ms、11ms 之间频繁跳动，带来高达 **10% 的时间量化噪声**，直接污染陀螺仪的积分环节。
* **极简优化**：实际工程中，强烈建议将源码中的 `millis()` 替换为 **`micros()`**（微秒计数器），系数改为 `1e-6f`。只需改这一行，解算精度就能提升一个数量级。

### 2. 裸机 vs FreeRTOS 架构选型
* **单纯 IMU 模块**：用**裸机 + micros ()** 算 `dt` 是最完美的。没有系统调度的开销，时序确定性最高。
* **多任务系统**：如果以后要加入蓝牙通信、多路电机 PID 控制等复杂任务，才引入 **FreeRTOS**。此时要采用“硬件定时器/IMU 中断 + 信号量唤醒任务”的架构，否则 RTOS 的任务切换会带来更大的执行时间抖动。