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