自動運転のカルマンフィルタ（センサー融合の数学）

Q: 自動運転のカルマンフィルタ（センサー融合の数学）とは何ですか？

時刻 t における真の位置を x_t とし、運動モデルによる予測値を \hat x_t^-（分散 \sigma_{\text{pred}}^2）、センサーによる観測値を z_t（分散 \sigma_{\text{obs}}^2）とする。1次元カルマンフィルタは、これら2つの情報を「分散の逆数（精度）」で重み付けした加重平均として最適推定値 \hat x_t を求める。 K = \frac{\sigma_{\text{pred}}^2}{\sigma_{\text{pred}}^2 + \sigma_{\text{obs}}^2} \quad (\text{カルマンゲイン}) \hat x_t = \hat x_t^- + K\big(z_t - \hat x_t^-\big) 更新後の推定分散は次式で必ず減少する。 \sigma_t^2 = (1 - K)\, \sigma_{\text{pred}}^2

Q: 自動運転のカルマンフィルタ（センサー融合の数学）は社会やプログラミングでどのように使われていますか？

自動運転車における1次元カルマンフィルタによるセンサーフュージョン: 運動モデルによる予測位置とGPS等のノイズを含む観測位置を、それぞれの分散に基づいてカルマンゲインで融合し、単独のセンサーや予測だけよりも真の位置に近い推定値を逐次的に計算する。実際の自動運転スタックでは、この考え方を多次元・非線形の状態空間モデル（拡張/無香料カルマンフィルタ）に拡張し、GPS・IMU・車輪速・カメラ・LiDARなど多数のセンサーを統合している。

ノイズだらけのセンサー値と予測モデルを「信頼度（分散）」に応じてブレンドする最適推定

この単元の発見をシェア

3行要約（Key Takeaways / AIOサマリー）

1: カルマンフィルタは「ノイズのあるセンサー測定値」と「運動モデルによる予測値」の両方を、それぞれの不確かさ（分散）に応じた重みで融合し、単独より精度の高い推定値を得る手法
2: 分散が小さい（信頼できる）情報ほど大きな重みが与えられ、分散が大きい（信頼できない）情報ほど軽視される「分散重み付き平均」が数学的な核心
3: 自動運転車はGPS・カメラ・IMU（慣性センサー）など複数のノイズを含むセンサー情報を、カルマンフィルタで統合して自車位置を高精度に推定している

なぜこれを学ぶのか？ — 現実社会とテクノロジーでの価値

Purpose & Motivation

なぜ生まれたのか？

カルマンフィルタは「ノイズのあるセンサー測定値」と「運動モデルによる予測値」の両方を、それぞれの不確かさ（分散）に応じた重みで融合し、単独より精度の高い推定値を得る手法

身につく武器・思考力

分散が小さい（信頼できる）情報ほど大きな重みが与えられ、分散が大きい（信頼できない）情報ほど軽視される「分散重み付き平均」が数学的な核心

実社会・ITでの活躍現場

•自動運転車における1次元カルマンフィルタによるセンサーフュージョン
•ITシステム・アルゴリズム実装における論理設計
•データ分析やシミュレーションにおける数理モデリング

Layer 1 — 直感・シミュレーター

「予測」（動きのモデルから計算した推定位置）と「観測」（ノイズを含むセンサーの生データ）をそれぞれ折れ線としてプロットし、両者を分散に応じてブレンドした「カルマン推定値」が、真の位置（正解ライン）にどれだけ近づくかを見てみよう。ノイズの大きさをスライダーで変え、「1ステップ進める」ボタンで時間を進めながら、フィルタが徐々に真の軌道を捉えていく様子を観察できる。

センサーのノイズの大きさ±3.0

真の位置ノイズ付きセンサー観測カルマン推定値

💡 分散重み付きブレンド:

カルマンゲイン $K = \dfrac{\sigma_{\text{pred}}^2}{\sigma_{\text{pred}}^2 + \sigma_{\text{obs}}^2}$ によって、予測と観測を不確かさに応じてブレンドしています。現在の推定分散: 5.00（t=0, 推定値=0.00）

↑ ノイズを大きくしてもカルマン推定値（青線）は真の位置（グレー線）に近い滑らかな軌道を保ちます。センサー単独（オレンジの破線）よりも安定して真の値を追跡できることを確認しよう。

直感チェック！ミニクイズ

カルマンフィルタが「センサーの測定値」と「予測モデルの値」を融合する際、より重視するのはどちらか？

Layer 2 — 定義・受験・証明

定義

時刻 $t$ における真の位置を $x_t$ とし、運動モデルによる予測値を $\hat x_t^-$ （分散 $\sigma_{\text{pred}}^2$ ）、センサーによる観測値を $z_t$ （分散 $\sigma_{\text{obs}}^2$ ）とする。1次元カルマンフィルタは、これら2つの情報を「分散の逆数（精度）」で重み付けした加重平均として最適推定値 $\hat x_t$ を求める。

K = \frac{\sigma_{\text{pred}}^2}{\sigma_{\text{pred}}^2 + \sigma_{\text{obs}}^2} \quad (\text{カルマンゲイン})

\hat x_t = \hat x_t^- + K\big(z_t - \hat x_t^-\big)

更新後の推定分散は次式で必ず減少する。

\sigma_t^2 = (1 - K)\, \sigma_{\text{pred}}^2

公式・数理モデル

K = \frac{\sigma_{\text{pred}}^2}{\sigma_{\text{pred}}^2 + \sigma_{\text{obs}}^2}, \quad \hat x_t = \hat x_t^- + K(z_t - \hat x_t^-)

証明・導出

分散重み付き平均が最尤推定（＝最も確からしい統合値）になることの導出：

予測値 $\hat x_t^-$ と観測値 $z_t$ が、それぞれ独立な正規分布 $\mathcal{N}(\hat x_t^-, \sigma_{\text{pred}}^2)$ 、 $\mathcal{N}(z_t, \sigma_{\text{obs}}^2)$ に従う真の値 $x$ についての2つの独立な情報だとみなす。ベイズの定理より、この2つの正規分布の尤度を掛け合わせた事後分布もまた正規分布になり（正規分布同士の積は正規分布に比例する）、その平均 $\mu^*$ は分散の逆数（精度）による重み付き平均で与えられる。

\mu^* = \frac{\dfrac{1}{\sigma_{\text{pred}}^2}\hat x_t^- + \dfrac{1}{\sigma_{\text{obs}}^2} z_t}{\dfrac{1}{\sigma_{\text{pred}}^2} + \dfrac{1}{\sigma_{\text{obs}}^2}}

この式を通分し、 $K = \sigma_{\text{pred}}^2 / (\sigma_{\text{pred}}^2 + \sigma_{\text{obs}}^2)$ と置いて整理すると、

\mu^* = \hat x_t^- + K(z_t - \hat x_t^-)

というカルマンフィルタの更新式に一致する（代数的に確認できる）。

また事後分布の分散 $\sigma^{*2}$ は、精度（分散の逆数）同士の和の逆数になるため、

\frac{1}{\sigma^{*2}} = \frac{1}{\sigma_{\text{pred}}^2} + \frac{1}{\sigma_{\text{obs}}^2} \ \Longrightarrow \ \sigma^{*2} = (1-K)\sigma_{\text{pred}}^2 < \sigma_{\text{pred}}^2

すなわち2つの情報源を統合すると、統合後の不確かさは必ずどちらか一方だけの不確かさより小さくなる（情報を捨てない限り不確かさは単調に減少する）。この「事前分布（予測）×尤度（観測）＝事後分布」という構造は、ベイズ推定の逐次更新そのものであり、カルマンフィルタは「線形・ガウス分布」という特別な条件下でベイズ更新を閉じた式で解けるようにしたアルゴリズムである。

押さえておきたいポイント

カルマンフィルタは「予測（Predict）」と「更新（Update）」を1ステップごとに繰り返す逐次推定アルゴリズムである
センサーのノイズが大きい（ $\sigma_{\text{obs}}^2$ が大きい）ほどカルマンゲイン $K$ は小さくなり、予測モデル側の値がより重視される
自動運転車ではGPS（大まかだが絶対位置が分かる）、IMU（高頻度だが誤差が蓄積する）、カメラ・LiDAR（周囲の物体は分かるが単独では絶対位置が曖昧）など、性質の異なる複数センサーをカルマンフィルタ（または拡張カルマンフィルタ、パーティクルフィルタ）で融合する「センサーフュージョン」が自己位置推定の中核技術
多次元・非線形な系に拡張したものが拡張カルマンフィルタ（EKF）・無香料カルマンフィルタ（UKF）であり、ロボティクスや航空宇宙分野でも広く使われている

参照基準・学習指導要領との対応

本トピックの内容・数理モデルは、文部科学省学習指導要領（算数・数学・情報）および国際標準カリキュラムに準拠し、エンジニアリング現場の実装事例と照らし合わせてMath World Lab編集部により検証・作成されています。

Layer 3 — 工学応用・現場実装

自動運転車における1次元カルマンフィルタによるセンサーフュージョン

運動モデルによる予測位置とGPS等のノイズを含む観測位置を、それぞれの分散に基づいてカルマンゲインで融合し、単独のセンサーや予測だけよりも真の位置に近い推定値を逐次的に計算する。実際の自動運転スタックでは、この考え方を多次元・非線形の状態空間モデル（拡張/無香料カルマンフィルタ）に拡張し、GPS・IMU・車輪速・カメラ・LiDARなど多数のセンサーを統合している。

1interface KalmanState {
2  estimate: number;   // 現在の推定位置
3  variance: number;   // 現在の推定の分散（不確かさ）
4}
5
6// 予測ステップ: 運動モデルで次の状態を予測する（ここでは等速直線運動を仮定）
7function predict(state: KalmanState, velocity: number, processNoise: number): KalmanState {
8  return {
9    estimate: state.estimate + velocity,
10    variance: state.variance + processNoise, // 予測を重ねるほど不確かさは増加する
11  };
12}
13
14// 更新ステップ: ノイズを含むセンサー観測値で予測を補正する
15function update(predicted: KalmanState, measurement: number, measurementVariance: number): KalmanState {
16  const kalmanGain = predicted.variance / (predicted.variance + measurementVariance);
17
18  return {
19    estimate: predicted.estimate + kalmanGain * (measurement - predicted.estimate),
20    variance: (1 - kalmanGain) * predicted.variance, // 統合後は必ず不確かさが減少
21  };
22}
23
24// シミュレーション: 真の位置は毎ステップ+2.0m進む等速運動、GPSにはノイズが乗る
25let state: KalmanState = { estimate: 0, variance: 5 };
26const trueVelocity = 2.0;
27let truePosition = 0;
28
29for (let t = 1; t <= 5; t++) {
30  truePosition += trueVelocity;
31  const noisyGpsReading = truePosition + (Math.random() - 0.5) * 4; // ノイズ付きGPS
32
33  const predicted = predict(state, trueVelocity, 0.1);
34  state = update(predicted, noisyGpsReading, 3.0);
35
36  console.log(
37    `t=${t}: 真の位置=${truePosition.toFixed(2)}, GPS観測=${noisyGpsReading.toFixed(2)}, カルマン推定=${state.estimate.toFixed(2)}`
38  );
39}