センサフュージョンとIMU

IMUは「3つの不完全なセンサ」を束ねる

IMU（慣性計測装置）は、加速度計・ジャイロスコープ・（多くは）地磁気センサを1つにまとめたデバイスです。目的は物体の姿勢（向き）、あわよくば位置を知ることにあります。ところが3つのセンサはどれも単独では姿勢を出しきれません。それぞれが得意な時間帯と苦手な誤差を持ち、しかもその弱点が互いに補完的だからこそ、融合（フュージョン）する価値が生まれます。

まず各センサが何を測り、何に弱いかを正確に押さえます。

決定的なのは、ジャイロと加速度計の誤差が正反対の周波数特性を持つ点です。ジャイロは短時間なら極めて正確ですが、わずかなバイアスを積分し続けるため誤差が時間に比例して増える（低周波で悪い）。加速度計は長時間平均すれば重力方向を正しく示しますが、運動加速度や振動が高周波の外乱として乗る（高周波で悪い）。この相補性こそがフュージョンの数学的な土台です。

座標系と姿勢の表現：オイラー角とクォータニオン

融合の前に、姿勢を数値でどう表すかを決めます。姿勢とは機体座標系（ボディフレーム）が基準座標系（多くは局所水平のNED＝北・東・下）に対してどれだけ回っているかで、代表的な表現が2つあります。

オイラー角はロール・ピッチ・ヨーの3角度で直感的ですが、致命的な弱点があります。ピッチが±90度に近づくと、ロール軸とヨー軸が同じ方向を向いてしまい、3自由度のうち1つが失われるジンバルロックが起きます。この特異点付近では姿勢が一意に定まらず、計算が発散します。

クォータニオンは4要素 (w, x, y, z) で3次元回転を表し、特異点を持ちません。単位クォータニオン（ノルムが1）が1つの回転に対応し、演算は乗算で連結できて数値的にも安定です。姿勢推定の実装がほぼクォータニオンを採るのはこのためです。

相補フィルタ：周波数で役割分担する最小構成

最も軽量な融合が相補フィルタです。発想は単純で、ジャイロ由来の角度をハイパスに、加速度計（と地磁気）由来の角度をローパスに通し、足し合わせます。両者のフィルタ特性は足すと1になる（相補的）ため、全帯域で正しい角度が残り、各センサの苦手帯域が打ち消されます。

相補フィルタ（1軸、係数 alpha は 0.95〜0.99 程度）
  angle = alpha * (angle + gyro_rate * dt) + (1 - alpha) * angle_accel

  第1項 : 前回角度にジャイロ積分を足す ＝ 高周波を信頼（短時間の追従）
  第2項 : 加速度計から求めた絶対角度   ＝ 低周波を信頼（ドリフト補正）

  alpha は時定数 tau と dt から   alpha = tau / (tau + dt)
  alpha が大きいほどジャイロ寄り（滑らかだがドリフト補正が遅い）

alpha はカットオフ周波数を決めるつまみです。大きくすればジャイロを長く信頼して滑らかになりますが、加速度計による絶対補正が効くまで時間がかかります。小さくすれば補正は速いが振動を拾いやすい。この一次元の調整で実用姿勢が得られるのが相補フィルタの強みで、計算量が小さく割込み駆動の高レート更新に向きます。実装上はサンプリング周期 dt の一定性が精度を左右するため、タイマ割込みで dt を確定させる設計が定石です（/embedded/interrupt-handling-isr/ と /embedded/real-time-wcet/ の周期一定化がそのまま効きます）。

カルマンフィルタ：誤差の分散まで推定する

相補フィルタの alpha は固定の勘所ですが、カルマンフィルタは「今どちらのセンサをどれだけ信じるべきか」を、誤差の統計（分散）から動的に決めます。姿勢推定は非線形なので、カルマン系で実際に使われるのはほぼ**拡張カルマンフィルタ（EKF）か、その派生のUKF（アンセンテッドカルマンフィルタ）**です。なお組み込みで広く使われるMadgwick／Mahonyフィルタはカルマン系ではなく、相補フィルタを非線形に拡張した軽量な代替解（勾配降下やPIフィードバックで姿勢誤差を補正）で、共分散を持たないぶん計算が軽い点が特徴です。

カルマンフィルタは2段のループで動きます。

肝はカルマンゲインです。予測の不確かさ（分散）が大きく観測が確からしいときはゲインを上げて観測へ寄せ、逆なら予測を保ちます。つまり相補フィルタの alpha が、状況に応じて最適な値へ自動調整されるイメージです。

ドリフトとバイアス補正：フュージョンが解く本丸

センサフュージョンが最終的に潰したいのはドリフトです。発生源を分けて対策を整理します。

• ジャイロバイアス：静止していても出力が0にならないオフセット。これを積分すると角度が時間に比例して増えます。起動時に静止状態で数百サンプル平均してバイアスを推定し差し引くのが基本で、さらにEKFで走行中も継続推定します。バイアスの安定性はアラン分散で評価され、平均化しても消えないバイアス不安定性がドリフトの下限を決めます。
• 加速度計による重力基準：長時間平均では運動加速度が相殺され重力方向へ収束するので、ロール・ピッチのドリフトを止める絶対基準になります。ただし持続的な旋回や振動下では基準が汚れるため、加速度ノルムが重力（約9.8 m/s²）から大きく外れる区間は補正を弱める工夫が使われます。
• 地磁気によるヨー基準：ヨー（方位）はジャイロ積分でしか追えずドリフトしますが、地磁気で絶対方位を与えて補正します。問題は磁場歪みで、機体固有のオフセット（ハードアイアン）と軸ごとの感度歪み（ソフトアイアン）をキャリブレーションで補正しないと方位がずれます。

ドリフトの本質（1軸ジャイロの角度誤差）
  真の角度 θ ≈ ∫ ω dt
  実測は ω_meas = ω_true + bias + noise
  積分すると    誤差 ≈ bias * t + （ランダムウォーク項）
  → bias が一定でも角度誤差は時間 t に比例して増える
  → 加速度計/地磁気の絶対基準で「低周波を押さえる」ことでのみ止まる

ここで融合の意味が明確になります。ジャイロ単独では bias·t の項が止められず、加速度計・地磁気単独では運動・磁場歪みで基準が揺れる。両者を周波数（相補フィルタ）あるいは分散（カルマンフィルタ）で結合し、各センサの弱い帯域を相手の強い帯域で置き換える――これがドリフトを封じる唯一の道筋です。なお、この生の角速度・加速度値そのものはI2C/SPIバス越しに読み出され、ADC取り込みや校正の作法は下流というより上流の前処理として効いてきます（/embedded/bus-i2c-spi-uart/、/embedded/sensor-interfacing/）。

まとめ：不完全なセンサを相補で完成させる

センサフュージョンの核心は、加速度計・ジャイロ・地磁気という単独では不完全な3つのセンサを、誤差の周波数特性が相補的である事実に基づいて結合し、互いの弱点を打ち消すことにあります。姿勢はジンバルロックを避けるためクォータニオンで持ち、毎更新で正規化する。融合はまず相補フィルタで alpha により高域＝ジャイロ・低域＝絶対基準に役割分担し、さらに精度と適応性が要るならEKFで誤差分散に基づく動的重み付けとジャイロバイアスの同時推定を行う。そしてこの全ての目的は、bias·t で増え続けるドリフトを絶対基準で止めることにあります。この仕組みは、周期の一定性を保つ/embedded/real-time-wcet/のタイミング設計と、生値を正しく取り込む/embedded/sensor-interfacing/の前処理の上に成り立っています。