Complementary Filter

• 类别: State estimation & navigation › Localisation
• 标签: state-estimation, imu, attitude-estimation, sensor-fusion, filtering

一种简单的估计器，对一个传感器做低通滤波、对另一个做高通滤波，从而融合两个噪声互补的传感器；常用于把陀螺仪和加速度计数据融合为姿态。

定义

互补滤波器（Complementary filter）把同一个量的两种测量结合起来，这两种测量的误差位于不同的频段。一个传感器在低频段可信，让它通过低通滤波器；另一个在高频段可信，让它通过高通滤波器；两个滤波器的选取使其之和为一，从而让真实信号不受改变地通过。经典的例子是用 IMU 进行姿态估计：对陀螺仪积分得到的角度平滑、响应快，但会因偏置而漂移；由加速度计计算出的倾角则噪声大，会受振动和加速度干扰，却不会漂移。把两者融合，就得到既响应快又稳定的横滚角和俯仰角。

在最简单的离散形式中，滤波器按 θ ← α(θ + ω·Δt) + (1 − α)·θ_acc 更新角度，其中接近 1 的 α 决定了交叉时间常数。非线性版本直接在旋转矩阵或四元数上运算，以便在大角度下依然有效；Mahony、Hamel 和 Pflimlin（2008）的显式互补滤波器还通过一个积分项估计陀螺仪偏置，而 Madgwick 滤波器则使用梯度下降修正步。航向无法由加速度计校正，因为重力不包含绕竖直轴旋转的任何信息，因此偏航需要磁力计、视觉或其他参考。

关键假设是：在时间平均意义上，加速度计测量的是对重力的反作用。加速度计实际测量的是比力，即机体加速度与重力之差，因此在持续加速期间，例如一次长时间的协调转弯，倾角参考会产生偏差。在飞行中的多旋翼上，水平方向的加速度计读数主要反映旋翼阻力而不是倾角，这限制了该假设的成立程度。与卡尔曼滤波器相比，互补滤波器增益固定，没有显式的不确定性模型，计算成本极低，这就是它在小型飞控和爱好者 IMU 板上很常见的原因。对于噪声平稳的线性问题，稳态卡尔曼滤波器具有相同的互补形式，只是增益由噪声模型选定。

为何重要

它往往是工程师实现的第一种传感器融合方法，在算力紧张或只需要一个简单、可调的姿态估计的场合，它仍是一个实用的选择。理解它的重力假设，也能解释为什么姿态估计在持续加速期间会变差。

主要应用

• 小型飞控和 IMU 板上的姿态估计
• 平衡机器人和倒立摆平台
• 相机云台和手持设备的倾角感测
• 融合气压计和加速度计数据以获得高度和爬升率
• 与更复杂的滤波器并行运行的备用姿态估计

实际应用

一台两轮平衡机器人在微控制器上的高速控制回路中读取一个六轴 IMU。用 atan2 从加速度计各轴计算出的俯仰角，每当电机猛拉底盘时就会跳变，而积分得到的陀螺仪角速率则稳定地漂移。工程师减去启动时测得的陀螺仪偏置，然后用互补滤波器把两者融合，所选的交叉时间常数既要足够长以忽略电机的猛拉，又要足够短以抵消漂移。同样的思路也出现在研究系统中：在关于旋翼失效后仍继续飞行的四旋翼的工作中，互补滤波器仅凭 IMU 提供俯仰角和横滚角，而机载视觉提供位置和航向。

常见误解

• 加速度计测量的是重力 — 它测量的是比力，只有当机体不在加速时，比力才等于重力的反作用
• 陀螺仪和加速度计足以给出完整的 3D 姿态 — 偏航无法通过重力观测，没有磁力计或其他航向参考就会漂移
• 互补滤波器是卡尔曼滤波器的粗糙替代品 — 线性问题的稳态卡尔曼滤波器具有相同的结构；区别在于后者的增益来自噪声模型

谁需要了解

• 嵌入式和飞控开发者
• 制作无人机和平衡机器人的爱好者
• 状态估计工程师
• 学习传感器融合的机器人专业学生

相关技术

• 卡尔曼滤波器（Kalman Filter）
• Mahony 滤波器（Mahony Filter）
• Madgwick 滤波器（Madgwick Filter）
• 航姿参考系统（Attitude and Heading Reference System，AHRS）
• 四元数积分（Quaternion Integration）

相关术语

• IMU
• Sensor Fusion
• Extended Kalman Filter
• Yaw / Pitch / Roll
• UAV
• Visual-Inertial Odometry

相关项目

• PCL: Industry-standard C++ library for point cloud processing in robotics perception
• OpenCat: An open-source Arduino and Raspberry Pi-based quadruped robot pet framework
• FAST-LIVO2: Fast, direct LiDAR-inertial-visual odometry from HKU-MARS, published in IEEE T-RO
• GTSAM: Factor-graph optimization library powering many modern SLAM backends
• OpenVINS: The reference open platform for visual-inertial state estimation research
• manif: Header-only C++ Lie theory library for robotics state estimation

视频

• Drone Control and the Complementary Filter: Brian Douglas
• Complementary Filter Sensor Fusion between Accelerometer and Gyroscope to estimate Pitch and Roll: OtherYATC