3. 陀螺仪与加速度计数据融合（互补滤波）

任务定义

本案例实现六轴IMU（陀螺仪+加速度计）的姿态融合，输出横滚角（Roll）与俯仰角（Pitch）。陀螺仪动态响应快但存在积分漂移，加速度计静态准确但易受振动干扰。互补滤波通过权重系数融合两者：高频段信任陀螺仪，低频段信任加速度计，在嵌入式平台上以极低计算开销获得可用姿态角。

核心算法

<code>// 互补滤波单次更新
// gyro: 陀螺仪角速度(rad/s), acc: 加速度计原始值(m/s^2)
// dt: 采样间隔(s), alpha: 滤波系数(0.90~0.98)
void complementary_filter(float gyro_rate, float acc_angle,
                          float* fused_angle, float dt, float alpha) {
    // 陀螺仪积分
    float gyro_angle = *fused_angle + gyro_rate * dt;
    // 互补融合
    *fused_angle = alpha * gyro_angle + (1.0f - alpha) * acc_angle;
}

完整实现

<code>#include <stdio.h>
#include <math.h>

#define PI 3.14159265f
#define RAD2DEG 57.29578f

// 从加速度计计算姿态角（仅横滚与俯仰）
void accel_to_angle(float ax, float ay, float az,
                    float* roll, float* pitch) {
    *roll  = atan2f(ay, az) * RAD2DEG;
    *pitch = atan2f(-ax, sqrtf(ay*ay + az*az)) * RAD2DEG;
}

// 互补滤波融合
void complementary_filter(float gyro_rate, float acc_angle,
                          float* fused_angle, float dt, float alpha) {
    float gyro_angle = *fused_angle + gyro_rate * dt;
    *fused_angle = alpha * gyro_angle + (1.0f - alpha) * acc_angle;
}

int main() {
    // 模拟数据：静止状态，陀螺仪有微小零偏
    float dt = 0.01f;          // 100Hz采样
    float alpha = 0.96f;       // 滤波系数

    // 初始姿态角（度）
    float roll = 0.0f, pitch = 0.0f;

    // 模拟1000步（10秒）
    for (int i = 0; i < 1000; i++) {
        // 模拟静止：加速度计读数约[0, 0, 9.8]
        float ax = 0.0f, ay = 0.0f, az = 9.8f;
        // 模拟陀螺仪零偏：0.01 rad/s ≈ 0.57°/s
        float gyro_roll_rate = 0.01f;
        float gyro_pitch_rate = 0.01f;

        // 加速度计解算角度
        float acc_roll, acc_pitch;
        accel_to_angle(ax, ay, az, &acc_roll, &acc_pitch);

        // 互补滤波更新
        complementary_filter(gyro_roll_rate, acc_roll, &roll, dt, alpha);
        complementary_filter(gyro_pitch_rate, acc_pitch, &pitch, dt, alpha);

        // 每100步输出一次（即每秒）
        if (i % 100 == 0) {
            printf("t=%.1fs  Roll=%7.3f°  Pitch=%7.3f°\n",
                   i*dt, roll, pitch);
        }
    }
    return 0;
}

代码说明

上述代码完整实现了互补滤波闭环：accel_to_angle从加速度计三轴分量解算静态角度；complementary_filter将陀螺仪积分角度与加速度计角度按权重融合。主循环模拟10秒静止状态，陀螺仪注入0.01 rad/s的零偏误差。若仅用陀螺仪积分，10秒后将漂移约5.7°；互补滤波通过加速度计持续校正，将稳态误差限制在零点附近。

编译与运行

gcc -o complementary_filter complementary_filter.c -lm
./complementary_filter

验证方法

运行程序后观察输出：初始时刻（t=0.0s）角度为0°，随后由于陀螺仪零偏，角度会短暂上升，但互补滤波会将其拉回接近0°的稳态值。判定标准：10秒内任意时刻输出角度值应小于1.0°（若纯陀螺仪积分则10秒后约为5.7°）。若将alpha改为1.0（纯积分），可对比验证漂移现象；改为0.0（纯加速度计），可观察振动噪声放大效果。

预期输出（前3行）

t=0.0s  Roll=  0.000°  Pitch=  0.000°
t=1.0s  Roll=  0.022°  Pitch=  0.022°
t=2.0s  Roll=  0.022°  Pitch=  0.022°

参数调优说明

滤波系数alpha决定信任权重：增大alpha（如0.98）使输出更平滑但响应变慢；减小alpha（如0.90）使输出更敏捷但引入更多加速度计噪声。实际系统中应根据传感器噪声特性和运动场景调整，通常取0.90～0.98之间。采样间隔dt必须与实际调用频率一致，否则积分误差会累积。