センサフュージョン — 不確かさを掛け合わせて縮める

前章で見たとおり、オドメトリは滑らかだがドリフトし、GPSはドリフトしないがガタガタ。互いの弱点を打ち消すように混ぜるのがセンサフュージョンで、その中心にいる道具がカルマンフィルタです。「不確かな方を弱く信じる」というたった一つの原理を、3つのデモで体に入れます。

1. 2つの不確かな情報の正しい混ぜ方

数直線上で自分の位置を推定している、としましょう。手元には2つの情報があります。

予測：オドメトリを足し込んだ「たぶんこの辺」— 平均 μ_pred、ばらつき σ_pred のガウス分布
観測：GPSが言ってきた「ここだ」— 平均 z、ばらつき σ_obs のガウス分布

単純に平均してはいけません。正解は分散の逆数で重み付けすること — 数学的には2つのガウス分布の掛け算です。掛け合わせた結果（事後分布）は、必ずどちらの分布よりも狭くなります。

1次元カルマン更新 — 予測 × 観測 = より確かな事後分布

予測のばらつき σ_pred

観測のばらつき σ_obs

水色＝予測（μ=−3）、オレンジ＝観測（z=+3）、緑＝掛け合わせた事後分布。2本のスライダーを動かすと、事後分布の山が確かな方（σが小さい方）に吸い寄せられ、しかもどちらよりも細く・高くなるのが分かります。上のバーは「どちらを何%信じたか」。

μ_new = μ_pred + K ( z − μ_pred ) K = σ²_pred / ( σ²_pred + σ²_obs ) K がカルマンゲイン。「観測をどれだけ信じるか」の割合（0〜1）で、予測が不確かなほど 1 に、観測が不確かなほど 0 に近づく

POINT — 不確かな方を弱く信じるカルマンフィルタの本体は、この重み付け平均がすべて。センサの値を「正しい/間違い」で二分するのではなく、それぞれの自信（分散）に比例した発言権を与える。だからこそ精度の違うセンサを何本でも同じ枠組みで混ぜられる。

実はこれ、ベイズの考え方で見た更新そのものです。予測が事前分布、観測が尤度、掛け算の結果が事後分布。カルマンフィルタとは「ガウス分布に限定したベイズ更新を、時々刻々逐次的に回し続ける仕組み」なのです。

2. 予測と更新のサイクル — 分布は膨らみ、縮む

走行中はこの更新が1回では終わりません。カルマンフィルタは2つのステップを交互に繰り返します。

予測ステップ：運動モデルで状態を進める。モデルは完璧ではないので（プロセスノイズ Q）、不確かさは増える。
更新ステップ：観測が届いたら掛け算で融合。観測にも誤差はあるが（観測ノイズ R）、不確かさは必ず減る。

予測: σ² ← σ² + Q 更新: σ² ← (1 − K) σ² 呼吸のように「膨らんで、縮む」を繰り返す。観測が来ない間は膨らむ一方になる

動く車の1次元追跡 — ±2σの帯が呼吸する

観測頻度

水色＝真の位置、緑＝フィルタの推定 μ、青い帯＝±2σ、オレンジの点＝観測。観測のたびに帯がキュッと縮み、次の観測までじわっと膨らみます。「観測を止める」を押すと、予測だけが続いて帯が育ち、推定が真の位置から流されていく様子（＝前章のデッドレコニング！）が見えます。

注意 — Q と R は設計者が決める「性格」プロセスノイズ Q を大きくする＝「自分の運動モデルを信じない」→ 観測に飛びつく敏感でガタガタなフィルタに。観測ノイズ R を大きくする＝「センサを信じない」→ 滑らかだが反応の鈍いフィルタに。この2つのさじ加減（チューニング）がカルマンフィルタ実務の半分を占める。

3. GPS + IMU の2Dフュージョン — いいとこ取りを見る

いよいよ2次元の周回コースで、前章の登場人物を全員混ぜます。(a) IMU/オドメトリだけ：滑らかだがドリフトする。(b) GPSだけ：ドリフトしないがガタガタ。(c) カルマンフィルタで融合：滑らかで、かつ正確。3本の軌跡とRMSE（平均二乗誤差の平方根）を見比べてください。

周回コースでの3軌跡比較 — オドメトリ / GPS / 融合

IMU/オドメトリノイズ

GPSノイズ σ

オレンジ＝オドメトリのみ（RMSEが育ち続ける）、赤＝GPS測位そのまま（RMSEはσ程度で頭打ちだがギザギザ）、緑＝融合。GPSノイズを上げると融合はGPSを信じなくなり（K↓）滑らかさで稼ぎ、IMUノイズを上げると観測に強く引き戻されます。どちらの単独よりRMSEが小さくなるのが融合の価値です。

POINT — 相補性こそフュージョンの価値 IMUの誤差は低周波（ゆっくり溜まるドリフト）、GPSの誤差は高周波（毎回ランダムなばらつき）。周波数帯で棲み分けているから、混ぜると両方の悪いところだけが消える。似たセンサを2本並べるより、誤差の性質が違うセンサを組むほうが効くのはこのため。

4. まとめ — カルマンフィルタは「自信の会計係」

融合の原理はガウス分布の掛け算＝分散の逆数による重み付け平均。カルマンゲイン K が「観測を信じる割合」。
走行中は予測（σ²が増える）→ 更新（σ²が減る）のサイクルを回し続ける。ベイズ更新の逐次版。
Q と R のチューニングがフィルタの性格を決める。誤差の性質が相補的なセンサの組み合わせほど効果が大きい。
同じ道具が自車位置だけでなく物体追跡（他車・歩行者の追跡）にもそのまま使われる。

一歩先へ — EKF・UKF・粒子フィルタここで扱ったのは線形・ガウスの世界。実際の車両は回転（cos・sin）を含む非線形システムなので、接線で線形近似するEKF（拡張カルマンフィルタ）や、代表点（シグマ点）を非線形変換に通すUKFが使われる。さらに「複数の候補地がある」ようなガウス分布で表せない状況では、仮説の粒をばらまく粒子フィルタの出番。次章のSLAMは、この推定問題に「地図作り」まで同時に背負わせる発展形だ。