前面讲了卡尔曼滤波——数学严谨但调参麻烦。在姿态估计领域有一种更简单实用的替代方案互补滤波。代码量只有卡尔曼的 1/5效果却不差。1. 为什么需要互补陀螺仪和加速度计测量的是同一个角度但噪声特征完全不同陀螺仪 (gyro) ✅ 短时间非常准不受振动影响 ❌ 积分会漂移——误差随时间累积1 分钟漂 5° 加速度计 (accel) ✅ 长时间平均是准的重力方向 ❌ 短时间噪声大——一振动就乱跳±5°互补滤波的思路陀螺仪 → 可信高频部分短时间内的角度变化加速度计 → 可信低频部分长时间的角度基准用一高一低两个滤波器把它们拼起来加速度计 → [低通] ─┐ ├→ 融合角度 陀螺仪 → [高通] ─┘但实际上高通 1 - 低通所以只需要一个低通滤波器融合角度 低通(加速度计角度) (1 - 低通)(陀螺仪积分角度)2. 一行公式搞定等价于这个递推公式angle α × (angle gyro × dt) (1 - α) × accel_angle 或者更直观的写法 angle (1 - α) × (angle gyro × dt) α × accel_angle └──── 陀螺仪占主导 ────┘ └── 加一点加速度计修正 ──┘α 的含义α 大 → 更信加速度计 → 收敛快但怕振动α 小 → 更信陀螺仪 → 平滑但怕漂移典型值α 0.022% 加速度计 98% 陀螺仪3. C 代码实现不到 30 行// comp_filter.h typedef struct { float angle; // 当前角度估计 float alpha; // 互补系数 (0.0 ~ 1.0) float dt; // 采样周期秒 } comp_filter_t; void comp_init(comp_filter_t *cf, float alpha, float dt); float comp_update(comp_filter_t *cf, float accel_angle, float gyro_rate);// comp_filter.c #include comp_filter.h void comp_init(comp_filter_t *cf, float alpha, float dt) { cf-alpha alpha; cf-dt dt; cf-angle 0; } float comp_update(comp_filter_t *cf, float accel_angle, float gyro_rate) { // 陀螺仪积分 → 预测下一秒的角度 float gyro_prediction cf-angle gyro_rate * cf-dt; // 互补融合大部分信陀螺仪小部分拉回加速度计 cf-angle (1.0f - cf-alpha) * gyro_prediction cf-alpha * accel_angle; return cf-angle; }4. 实战MPU6050 全姿态Roll Pitchcomp_filter_t roll_cf, pitch_cf; void imu_init(void) { mpu6050_init(); comp_init(roll_cf, 0.02f, 0.005f); // α0.02, dt5ms comp_init(pitch_cf, 0.02f, 0.005f); } void imu_read(float *roll, float *pitch) { float ax, ay, az, gx, gy, gz; mpu6050_read_all(ax, ay, az, gx, gy, gz); // 加速度计推算角度 float accel_roll atan2f(ay, az); float accel_pitch atan2f(-ax, sqrtf(ay*ay az*az)); // 陀螺仪速率rad/s直接传给互补滤波 *roll comp_update(roll_cf, accel_roll, gx); *pitch comp_update(pitch_cf, accel_pitch, gy); }α 值调参实验α 值抗振动抗漂移收敛速度推荐0.005极好差漂移明显慢飞行器悬停0.02好好中推荐默认0.05中好快快速运动0.1差好很快仅初始对准时用5. 互补滤波 vs 卡尔曼滤波互补滤波卡尔曼滤波代码量15 行80 行调参1 个参数 α2 个矩阵 Q, RCPU 开销极小中等矩阵运算精度够用±1°更高±0.5°理论最优❌ 不是✅ 是线性高斯假设下选择建议做平衡车、云台、航模 →互补滤波够用且简单做无人机导航、机器人定位 →卡尔曼精度要求高下一篇限幅滤波 消抖滤波——两种轻量级方案各 10 行代码
前面讲了卡尔曼滤波——数学严谨但调参麻烦。在姿态估计领域有一种更简单实用的替代方案互补滤波。代码量只有卡尔曼的 1/5效果却不差。1. 为什么需要互补陀螺仪和加速度计测量的是同一个角度但噪声特征完全不同陀螺仪 (gyro) ✅ 短时间非常准不受振动影响 ❌ 积分会漂移——误差随时间累积1 分钟漂 5° 加速度计 (accel) ✅ 长时间平均是准的重力方向 ❌ 短时间噪声大——一振动就乱跳±5°互补滤波的思路陀螺仪 → 可信高频部分短时间内的角度变化加速度计 → 可信低频部分长时间的角度基准用一高一低两个滤波器把它们拼起来加速度计 → [低通] ─┐ ├→ 融合角度 陀螺仪 → [高通] ─┘但实际上高通 1 - 低通所以只需要一个低通滤波器融合角度 低通(加速度计角度) (1 - 低通)(陀螺仪积分角度)2. 一行公式搞定等价于这个递推公式angle α × (angle gyro × dt) (1 - α) × accel_angle 或者更直观的写法 angle (1 - α) × (angle gyro × dt) α × accel_angle └──── 陀螺仪占主导 ────┘ └── 加一点加速度计修正 ──┘α 的含义α 大 → 更信加速度计 → 收敛快但怕振动α 小 → 更信陀螺仪 → 平滑但怕漂移典型值α 0.022% 加速度计 98% 陀螺仪3. C 代码实现不到 30 行// comp_filter.h typedef struct { float angle; // 当前角度估计 float alpha; // 互补系数 (0.0 ~ 1.0) float dt; // 采样周期秒 } comp_filter_t; void comp_init(comp_filter_t *cf, float alpha, float dt); float comp_update(comp_filter_t *cf, float accel_angle, float gyro_rate);// comp_filter.c #include comp_filter.h void comp_init(comp_filter_t *cf, float alpha, float dt) { cf-alpha alpha; cf-dt dt; cf-angle 0; } float comp_update(comp_filter_t *cf, float accel_angle, float gyro_rate) { // 陀螺仪积分 → 预测下一秒的角度 float gyro_prediction cf-angle gyro_rate * cf-dt; // 互补融合大部分信陀螺仪小部分拉回加速度计 cf-angle (1.0f - cf-alpha) * gyro_prediction cf-alpha * accel_angle; return cf-angle; }4. 实战MPU6050 全姿态Roll Pitchcomp_filter_t roll_cf, pitch_cf; void imu_init(void) { mpu6050_init(); comp_init(roll_cf, 0.02f, 0.005f); // α0.02, dt5ms comp_init(pitch_cf, 0.02f, 0.005f); } void imu_read(float *roll, float *pitch) { float ax, ay, az, gx, gy, gz; mpu6050_read_all(ax, ay, az, gx, gy, gz); // 加速度计推算角度 float accel_roll atan2f(ay, az); float accel_pitch atan2f(-ax, sqrtf(ay*ay az*az)); // 陀螺仪速率rad/s直接传给互补滤波 *roll comp_update(roll_cf, accel_roll, gx); *pitch comp_update(pitch_cf, accel_pitch, gy); }α 值调参实验α 值抗振动抗漂移收敛速度推荐0.005极好差漂移明显慢飞行器悬停0.02好好中推荐默认0.05中好快快速运动0.1差好很快仅初始对准时用5. 互补滤波 vs 卡尔曼滤波互补滤波卡尔曼滤波代码量15 行80 行调参1 个参数 α2 个矩阵 Q, RCPU 开销极小中等矩阵运算精度够用±1°更高±0.5°理论最优❌ 不是✅ 是线性高斯假设下选择建议做平衡车、云台、航模 →互补滤波够用且简单做无人机导航、机器人定位 →卡尔曼精度要求高下一篇限幅滤波 消抖滤波——两种轻量级方案各 10 行代码
