📡

カルマンフィルタについて

に公開
現代制御
tech

概要

よく忘れるので記事にしました

  • ノイズを含む観測データとシステムのモデルを組み合わせて、真値を逐次的に推定するアルゴリズム
  • 動的なシステムにおいて、現在の状態を最も確からしく推定するために使われる
  • とくに、センサーデータや測位、追跡などに広く使われる

基本概念

カルマンフィルタは以下の2つのステップで構成される

  1. 予測（Prediction）
  • 前の状態と運動モデルから、現在の状態を予測
  1. 更新（Update / Correction）
  • 観測データを使って、予測を補正
  • 観測の信頼性と予測の信頼性を考慮して、よりよい推定値を計算

数式モデル（離散時間）

状態を表すベクトルを x_k、観測値を z_k としたとき、以下のような式で表される

状態遷移（予測ステップ）

x_k = A * x_{k-1} + B * u_k + w_k
  • A: 状態遷移モデル（前の状態がどう変化するか）
  • B: 制御入力モデル
  • u_k: 制御入力
  • w_k: プロセスノイズ（平均0、分散Q）

観測モデル（更新ステップ）

z_k = H * x_k + v_k
  • H: 観測モデル（状態がどう観測されるか）
  • v_k: 観測ノイズ（平均0、分散R）

ポイント

  • ノイズのある観測
    • センサーや測定には誤差（ノイズ）が含まれる。例：GPSの位置ずれなど
  • 状態モデル（予測）
    • 対象の動き方や振る舞いに関する数理モデル（速度、加速度などから位置を予測）
  • 推定（フィルタリング）
    • 観測とモデルの両方を使って「現在の状態」を最も確からしく推定する
  • 逐次処理
    • データが1つ来るたびにリアルタイムで更新できる（バッチ処理ではない）

以下のような状況に強い

  • センサーが不正確でもOK
    • たとえばノイズの多い加速度センサーでも、正確な位置や速度が推定できる。
  • 観測が一時的に途切れてもOK
    • 過去の情報と予測モデルがあるから、それなりに良い状態を保てる。
  • リアルタイムで使える
    • 更新が速く、計算コストが低い

ローパスフィルタやハイパスフィルタとの違い

項目 カルマンフィルタ ローパスフィルタ / ハイパスフィルタ
原理 観測と予測モデルの統合 信号の周波数成分に対する単純なフィルタリング
使い方 センサーデータの状態推定、追跡など ノイズ除去、変化の強調など
設計 システムモデル（運動方程式など）が必要 カットオフ周波数を設定するだけ
対応 ノイズ・欠測・制御入力などすべて考慮できる ノイズ除去のみ、予測や制御は不可
次元数 多次元でも扱える（例：位置・速度・加速度） 通常は1次元信号に対して使う
例 自動運転の車両の位置推定 加速度センサのノイズ除去

実装例

import numpy as np

# 初期化
x = np.array([[0], [0]])  # 初期状態（位置, 速度）
P = np.eye(2) * 1000      # 共分散行列
A = np.array([[1, 1], [0, 1]])  # 状態遷移行列
H = np.array([[1, 0]])    # 観測モデル
R = np.array([[1]])       # 観測ノイズ
Q = np.eye(2) * 0.1       # プロセスノイズ
I = np.eye(2)             # 単位行列

# 観測データ
z_list = [1, 2, 3, 4, 5]

for z in z_list:
    # 予測
    x = A @ x
    P = A @ P @ A.T + Q

    # 更新
    z = np.array([[z]])
    y = z - H @ x
    S = H @ P @ H.T + R
    K = P @ H.T @ np.linalg.inv(S)
    x = x + K @ y
    P = (I - K @ H) @ P

    print("推定状態:", x.ravel())

Discussion