Kalman Filter (KF)

カルマンフィルター（KF）は、ノイズを含む測定値のストリームから、物体の位置や速度など、変化するシステムの隠れた状態を推定する再帰アルゴリズムです。各タイムステップで、システムの動きのモデルから次の状態を予測し、新しい測定値で予測を補正します。予測と測定値には、それぞれの不確実性に応じて重みを付けます。Rudolf E. Kálmán が 1960 年にこの手法を発表し、ガウスノイズを伴う線形システムでは、平均二乗誤差が最小の推定値を得られます。この手法は GPS 受信機、航空機や宇宙機の航法、ロボットの自己位置推定、そしてコンピュータービジョンにおけるほぼすべての複数物体追跡器で使われており、検出の揺らぎを平滑化し、各物体が次に現れる位置を予測します。

カルマンフィルターの仕組み#

フィルターは 2 つの量を保持します。たとえば位置と速度を表す状態推定値 x と、その推定値の不確実性を表す共分散行列 P です。各サイクルは 2 つのステップで構成されます。

1. 予測: 運動モデル F が状態を先の時点へ投影し、x = F x、モデルで捉えられない動きを表すプロセスノイズ Q によって不確実性が増加します。P = F P Fᵀ + Q
2. 更新: 新しい測定値 z が到着し、測定モデル H によって状態と関連付けられます。フィルターは残差 z − H x とカルマンゲイン K = P Hᵀ (H P Hᵀ + R)⁻¹ を計算します。ここで R は測定ノイズです。補正後の状態は x + K (z − H x) となり、不確実性は (I − K H) P まで小さくなります。

カルマンゲインは、モデルとセンサーのどちらを重視するかを決めます。測定値の精度が予測に比べて高い場合、ゲインは 1 に近づき、推定値は測定値に追従します。測定値にノイズが多い場合、ゲインは 0 に近づき、推定値は予測に追従します。状態には直接測定されない量も含められるため、フィルターはそのような量も推定します。位置だけを観測するトラッカーでも速度を推定できます。測定値が欠落した場合、フィルターは更新をスキップし、予測に基づいて進みます。

物体追跡におけるカルマンフィルター#

物体追跡では、各トラックが独自のカルマンフィルターを持ちます。状態にはバウンディングボックスの中心とサイズ、およびそれらの速度が含まれ、等速モデルが次のフレームでのボックス位置を予測します。予測位置はIntersection over Union（IoU）によって新しい検出と照合され、マッチした各検出がトラックを更新する測定値になります。物体が数フレーム隠れても、トラックは予測経路に沿って移動し続けるため、再び現れた検出は予測位置に十分近くなり、古い ID を維持できます。

Ultralytics YOLOのtrack モードにおけるすべてのトラッカーが、この方法で動作します。ByteTrack の KalmanFilterXYAH は、ボックス中心、アスペクト比、高さ、およびそれらの速度からなる 8 次元状態を追跡します。BoT-SORT と TrackTrack は幅と高さを使う変種で、どちらもボックスサイズに応じてノイズをスケーリングするため、近くにある大きな物体ではピクセル単位の大きな移動が許容されます。ボックス中心の 1 つの座標について、同じ予測と更新のステップを NumPy で数行に実装できます。

import numpy as np

F = np.array([[1.0, 1.0], [0.0, 1.0]])  # constant-velocity model, state = [position, velocity]
H = np.array([[1.0, 0.0]])  # the detector measures position only
Q = 0.001 * np.eye(2)  # process noise: how far real motion may stray from the model
R = np.array([[25.0]])  # measurement noise: detections jitter by about 5 px

rng = np.random.default_rng(0)
truth = 100 + 3.0 * np.arange(60)  # box center moving 3 px per frame
z = truth + rng.normal(0, 5.0, truth.size)  # noisy detections

x, P = np.array([z[0], 0.0]), np.diag([25.0, 100.0])  # start from the first detection
estimates = [x[0]]
for measurement in z[1:]:
    x, P = F @ x, F @ P @ F.T + Q  # predict
    K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R)  # Kalman gain
    x = x + K @ (measurement - H @ x)  # update with the weighted residual
    P = (np.eye(2) - K @ H) @ P
    estimates.append(x[0])

print(f"Detection error: {np.sqrt(np.mean((z - truth) ** 2)):.2f} px")
print(f"Kalman error: {np.sqrt(np.mean((np.array(estimates) - truth) ** 2)):.2f} px")
print(f"Estimated velocity: {x[1]:.2f} px/frame")

出力される二乗平均平方根誤差は、未処理の検出で 4.50 px、フィルター処理後の推定値で 2.48 px です。また、速度を一度も測定していないにもかかわらず、真の値である 3 px/フレームに近い 2.98 px/フレームの推定速度が得られます。

関連するフィルター#

標準的なカルマンフィルターは、ガウスノイズを伴う線形の運動モデルと測定モデルを仮定します。関連手法では、これらの仮定を緩和します。

• 拡張カルマンフィルター（EKF）: 各ステップでヤコビアンを使って現在の推定値の周りで非線形モデルを線形化する手法で、GPS や慣性航法で標準的に使われます。
• 無香料カルマンフィルター（UKF）: 慎重に選んだ少数のサンプル点を非線形モデルに通す手法です。通常、線形化よりも精度が高く、微分を必要としません。
• パーティクルフィルター: 多数の重み付きランダムサンプルで状態分布を表します。そのため、ロボットが 2 つの通路のどちらにいるか判断できない場合のように、非ガウス分布や多峰性の信念状態を扱えますが、計算コストは大幅に増加します。
• 隠れマルコフモデル（HMM）: 離散状態に対して同じ予測・更新の再帰処理を使います。カルマンフィルターは、連続かつ線形ガウス分布の状態に対応する手法です。

追跡以外の用途#

• ロボティクスとセンサーフュージョン: ロボットは車輪オドメトリー、IMU、ライダー、カメラを統合して 1 つの姿勢推定値を得ます。EKF ベースの手法は、同時自己位置推定とマッピングの古典的なアプローチです。
• 拡張現実: ヘッドセットは頭部姿勢の測定値をフィルター処理し、低レイテンシで仮想オブジェクトを安定させます。
• 経済学と信号処理: アナリストはカルマン平滑化を使い、ノイズを含む時系列から観測されていないトレンドを推定します。
• ナビゲーション: 航空機、船舶、ドローン、スマートフォンは GPS と慣性センサーを融合し、衛星測位の合間も位置を滑らかに保ちます。ナビゲーションはこのフィルターが最初に大きな成果を上げた分野でもあります。1960 年代初頭、NASA Ames の Stanley F. Schmidt がアポロの航法コンピューター向けに非線形軌道推定へ応用しました。