Complementary Filter

• カテゴリ: State estimation & navigation › Localisation
• タグ: state-estimation, imu, attitude-estimation, sensor-fusion, filtering

一方をローパス、もう一方をハイパスでフィルタリングすることで、ノイズが相補的な 2 つのセンサーを統合する単純な推定器。ジャイロスコープと加速度計のデータを融合して姿勢を得るのによく使われる。

定義

相補フィルタ（Complementary filter）は、誤差が異なる周波数帯にある同じ量の 2 つの計測を組み合わせる。一方のセンサーは低周波で信頼してローパスフィルタに通し、もう一方は高周波で信頼してハイパスフィルタに通す。2 つのフィルタは和が 1 になるように選び、真の信号が変化せずに通過するようにする。典型例は IMU による姿勢推定である。ジャイロスコープを積分すると滑らかで応答の速い角度が得られるがバイアスのためにドリフトし、加速度計から計算した傾きはノイズが多く振動や加速度に乱されるがドリフトしない。両者を混合すると、応答が速くかつ安定したロールとピッチが得られる。

最も単純な離散形では、フィルタは角度を θ ← α(θ + ω·Δt) + (1 − α)·θ_acc で更新し、1 に近い α がクロスオーバーの時定数を決める。非線形版は大きな角度でも成り立つよう回転行列や四元数の上で直接計算する。Mahony、Hamel、Pflimlin（2008）の明示的相補フィルタは積分項によってジャイロスコープのバイアスも推定し、Madgwick のフィルタは勾配降下法による補正ステップを使う。方位は加速度計では補正できない。重力には鉛直軸まわりの回転についての情報が含まれないため、ヨーには磁気センサー、ビジョン、その他の参照が必要である。

重要な仮定は、時間平均すれば加速度計は重力に対する反力を計測している、というものである。加速度計が実際に計測するのは比力、すなわち機体の加速度と重力の差であるため、長い協調旋回のような持続的な加速中は傾きの参照に偏りが生じる。飛行中のマルチロータでは、水平方向の加速度計の読みは傾きよりも主にロータの抗力を反映するため、この仮定が成り立つ度合いは限られる。カルマンフィルタと比べると、相補フィルタはゲインが固定で、明示的な不確かさのモデルを持たず、計算コストが非常に低い。小型のフライトコントローラや趣味用の IMU ボードでよく使われているのはこのためである。定常ノイズを持つ線形問題では、定常カルマンフィルタも同じ相補的な形をとり、ゲインはノイズモデルから選ばれる。

重要な理由

エンジニアが最初に実装するセンサーフュージョン手法であることが多く、計算資源が厳しい場合や、単純で調整可能な姿勢推定で十分な場合には今も実用的な選択肢である。その重力に関する仮定を理解すれば、持続的な加速中に姿勢推定が劣化する理由もわかる。

主な用途

• 小型フライトコントローラや IMU ボードでの姿勢推定
• 倒立二輪ロボットや倒立振子のプラットフォーム
• カメラジンバルやハンドヘルド機器の傾き検出
• 気圧計と加速度計のデータ融合による高度と上昇率の推定
• より複雑なフィルタと並行して動かす予備の姿勢推定

実際の使われ方

二輪の倒立ロボットが、マイコン上の高速な制御ループの中で 6 軸 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