1
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@NovaSolver

カルマンフィルターで「見えない状態」をリアルタイム推定してみた

1
Posted at

「GPSって、なんでピタッと止まってるのに位置がフラフラ動くの？」

スマホのマップアプリで青い点がプルプル震える経験、誰にでもあるはずです。あれ、実は衛星からの信号に含まれるノイズが原因。でも、車のナビはなぜあんなに滑らかに動くのか？

答えは「カルマンフィルター」という1960年発明の古典的アルゴリズムにあります。しかも、その本質はたった2つのパラメータで決まる——この事実、知ってました？

ざっくり本質：センサーと予測モデル、どっちを信じる？

カルマンフィルターを一言で表すならこうです：

「予測モデル」と「観測値」の間で、確率論的に最も確からしい値を計算する仕組み

イメージは「2人の証人の証言を、それぞれの信用度で重み付けして平均する」感じ。片方の証人がいつも酔っ払ってる（観測ノイズ大）なら、もう片方の冷静な証言（予測モデル）を重視する——これが本質です。

重要なのは、この「信用度」が時間とともに自動更新される点。最初は予測を信じていても、観測が何度も続けば「あれ、思ったよりセンサー正確だな」と学習して重みを変える。これが単なる移動平均との決定的な違いです。

数式で理解する：たった2行の更新則

カルマンフィルターの心臓部は、以下の予測ステップと更新ステップの2つだけ。1次元の場合、行列すら不要です。

予測ステップ（「次はここら辺にいるはず」）

\hat{x}_{k|k-1} = \hat{x}_{k-1|k-1} + \Delta t \cdot v

P_{k|k-1} = P_{k-1|k-1} + Q

更新ステップ（「観測が来たから修正」）

K_k = \frac{P_{k|k-1}}{P_{k|k-1} + R}

\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k (z_k - \hat{x}_{k|k-1})

P_{k|k} = (1 - K_k) P_{k|k-1}

各変数の意味：

  • $\hat{x}$: 推定したい状態（例：位置）
  • $P$: 推定の不確かさ（分散）——小さいほど自信あり
  • $Q$: プロセスノイズ——「予測モデルがどれだけ外れる可能性があるか」
  • $R$: 観測ノイズ——「センサーの測定誤差」
  • $K$: カルマンゲイン——予測と観測の「信用比率」、0〜1の値
  • $z_k$: 実際の観測値

直感的解釈: $K$が1に近いほど観測を重視、0に近いほど予測を重視。$K$は$P$と$R$の大小で自動決定されます。

コードで実装する：たった25行で動く

ここがこの記事の核心。上の数式をそのままJavaScriptに落とします。

class KalmanFilter1D {
  constructor(initialX, initialP, Q, R) {
    this.x = initialX; // 状態推定値
    this.P = initialP; // 推定誤差共分散
    this.Q = Q;        // プロセスノイズ
    this.R = R;        // 観測ノイズ
  }

  // 予測ステップ: 等速直線運動モデル (速度vは外部から与える)
  predict(dt, v = 0) {
    this.x += dt * v;           // 位置を更新
    this.P += this.Q;           // 不確かさが増加
  }

  // 更新ステップ: 観測値zで修正
  update(z) {
    const K = this.P / (this.P + this.R); // カルマンゲイン
    this.x += K * (z - this.x);           // 観測と予測の差を反映
    this.P = (1 - K) * this.P;            // 不確かさを減少
  }

  // 現在の状態を取得
  getState() {
    return { x: this.x, P: this.P };
  }
}

// 使用例: 真値=50、ノイズあり観測、Q=0.1, R=1.0
const kf = new KalmanFilter1D(0, 100, 0.1, 1.0);
const trueValue = 50;
for (let t = 0; t < 10; t++) {
  const observation = trueValue + (Math.random() - 0.5) * 2; // ノイズ±1
  kf.predict(1.0, 0);  // 等速モデル、速度0
  kf.update(observation);
  console.log(`t=${t}: 観測=${observation.toFixed(2)}, 推定=${kf.x.toFixed(2)}`);
}

動作の流れ:

  1. 初期状態 x=0、不確かさ P=100（全然わかってない）
  2. 予測で位置は変わらず、不確かさがQ分増える
  3. 観測が来たら、カルマンゲインKで「どの程度観測を信じるか」を計算
  4. 観測値と予測値の差 (z - x) にKをかけて補正
  5. 不確かさPが減少（情報が増えたから）

実際に動かすと: 最初の数ステップで推定値が真値50付近に収束。その後はノイズに振り回されず、滑らかに追従します。

数値例で確かめる：手計算で追う

具体的な数値を代入して、1ステップずつ計算してみましょう。

設定:

  • 初期状態: $\hat{x}{0|0} = 0$ (m), $P{0|0} = 100$ (m²)
  • パラメータ: $Q = 0.1$ (m²), $R = 1.0$ (m²)
  • 速度: $v = 0$ (停止状態を想定)
  • 観測値: $z_1 = 49.3$ (m) （真値50mにノイズが乗った）

ステップ1: 予測

\hat{x}_{1|0} = 0 + 1 \cdot 0 = 0 \text{ m}

P_{1|0} = 100 + 0.1 = 100.1 \text{ m}^2

ステップ2: 更新

K_1 = \frac{100.1}{100.1 + 1.0} = \frac{100.1}{101.1} \approx 0.9901

\hat{x}_{1|1} = 0 + 0.9901 \times (49.3 - 0) \approx 48.81 \text{ m}

P_{1|1} = (1 - 0.9901) \times 100.1 \approx 0.991 \text{ m}^2

結果の解釈:

  • カルマンゲインが約0.99と非常に大きい → 観測をほぼそのまま信じる
  • 理由: 初期の不確かさPが100と巨大で、観測ノイズR=1より圧倒的に大きいから
  • 推定値は48.81mと、観測値49.3mに近い（まだ真値50mには届かず）

ステップ2（続き）: 次の観測 $z_2 = 51.2$ m

予測: $\hat{x}{2|1} = 48.81$, $P{2|1} = 0.991 + 0.1 = 1.091$
更新: $K_2 = \frac{1.091}{1.091 + 1.0} \approx 0.5217$
推定: $\hat{x}{2|2} = 48.81 + 0.5217 \times (51.2 - 48.81) \approx 50.06$ m
不確かさ: $P{2|2} = (1 - 0.5217) \times 1.091 \approx 0.522$ m²

注目ポイント:

  • 2ステップ目でカルマンゲインが0.52に低下 → 予測と観測を半々で信用
  • 推定値が50.06mと、ほぼ真値に収束
  • 不確かさPが0.522まで減少 → 「だいたい0.7mくらいの精度でわかってる」状態

このように、時間とともにフィルターが「学習」し、適切なバランスを自動調整するのがカルマンフィルターの真骨頂です。

シミュレーターで遊ぶ：3つの実験

ここからは実際のシミュレーターで体感してください。
▶ カルマンフィルターシミュレーター （ブラウザで即動作、登録不要）

実験1: Rを極端に変えてみる

  • 設定: Q=0.1固定、Rを0.1→10→100と変更
  • 結果:
    • R=0.1: 推定値（青線）が観測値（赤点）にピッタリ追従。ノイズもそのまま通す
    • R=10: 推定値が滑らかに。観測のばらつきをうまく除去
    • R=100: 推定値がほぼ直線に。観測をほとんど無視、予測モデルに支配される

なぜ？: Rが大きい = 「センサーは信用できない」と判断 → 予測モデルを重視 → 変化に鈍感になる

実験2: Qを極端に変えてみる

  • 設定: R=1固定、Qを0.01→1→100と変更
  • 結果:
    • Q=0.01: 推定値がガチガチに固まる。急な変化に追従できない
    • Q=1: 適度に柔軟。ノイズは除去しつつ変化には対応
    • Q=100: 推定値が観測値にガクガク追従。ノイズをそのまま通す

なぜ？: Qが大きい = 「予測モデルは当てにならない」と判断 → 観測を重視 → ノイズに敏感になる

実験3: 「モデルが間違ってる」状況を作る

  • 設定: 真値（緑線）が途中で急カーブするよう、シミュレーターの軌道を変更
  • 条件A: Q=0.01, R=1 → 推定値が曲がり角で大遅れ、元の直線を信じ続ける
  • 条件B: Q=10, R=1 → 曲がり角でも素早く追従、ただし平常時はややガタつく

実務の教訓: 「普段は滑らかに、でも急変には追従したい」というトレードオフ。Qを適度に大きく設定するのがコツ。

現場でハマるポイント：3つの落とし穴

1. 発散（Divergence）—— 最悪のシナリオ

シミュレーターで「Qを0.001、Rを0.01」に設定し、真値を急に曲げてみてください。推定値（青線）が真値（緑線）からどんどん離れ、二度と戻ってこなくなります。

原因: 不確かさPが小さくなりすぎ、「予測が正しい」と過信。新しい観測を無視し始める。一度外れると、Pが小さいせいで観測との差を「ノイズ」と誤認し、修正できなくなる。

対策:

  • Qに下限を設定（例：システムの想定最大加速度から計算）
  • 適応的にQを調整する手法（Innovation Adaptive Estimation）を使う
  • 定期的にPをリセットする仕組みを入れる

2. パラメータチューニングの難しさ

「Rはセンサーデータシートから決められるが、Qは決められない」——これが実務の現実です。

実践的なアプローチ:

  1. まずRをデータシートの値に固定
  2. Qを大きめに設定（想定最大誤差の2〜3倍）
  3. シミュレーションで応答を確認しながらQを絞る
  4. 実機テストで微調整

3. 非線形システムへの対応

このシミュレーターは等速直線運動という線形モデル。しかし現実の多くは非線形（例：振り子の運動、車の旋回）。

  • 拡張カルマンフィルター（EKF）：非線形関数をテイラー展開で線形近似
  • Unscented Kalman Filter（UKF）：シグマ点を使って確率分布を近似
  • Particle Filter：完全な非線形・非ガウスに対応（ただし計算コスト大）

教訓: 「まず線形で理解し、その後拡張する」のが王道。このシミュレーターで1次元の動作原理を完全に理解すれば、EKFやUKFへのステップアップは格段に楽になります。

まとめ：今日から使える3つのポイント

  1. カルマンフィルターは「予測と観測の重み付け平均」 — その重み（カルマンゲインK）が自動最適化されるのが最大の強み
  2. QとRのバランスが全て — Q大＝観測重視、R大＝予測重視。実務ではRを先に決め、Qをチューニング
  3. 1次元の理解が多次元への鍵 — 行列演算に拡張しても、本質は同じ。このシミュレーターで直感を鍛えよう

最後に: 理論だけ読んでも身につきません。以下のシミュレーターで、実際にスライダーを動かしながら「Qを上げたらどうなる？」「Rを下げたら？」を体感してください。

▶ カルマンフィルターシミュレーター — ブラウザで即動作、登録不要

NovaSolverでは700以上の工学シミュレーターを無料公開中 👉 一覧はこちら

1
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
1
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?