カルマンフィルタ早見表

この早見表は、オドメトリ、慣性計測装置（IMU）を用いたセンサフュージョン、追跡のためにカルマンフィルタを書いたりデバッグしたりする人向けです。よく使われる3つの変種を並べて比較し、ノイズ行列の決め方と、フィルタが一貫しているかをデータから判断する方法を示します。背景はセンサとノイズと状態推定ハブにあります。用語集: カルマンフィルタ、拡張カルマンフィルタ、粒子フィルタ。

記法

記号	サイズ	意味
x, P	n, n x n	状態推定値とその共分散
F, B, u	n x n, n x m, m	線形の運動モデルと制御入力
f(x, u)	n	非線形の運動モデル（EKF、UKF）
Q	n x n	プロセスノイズ共分散。1ステップあたりにモデルが外す分
z, H, h(x)	p, p x n, p	観測、線形観測モデル、非線形観測モデル
R	p x p	観測ノイズ共分散
y, S, K	p, p x p, n x p	イノベーション、その共分散、カルマンゲイン

KF・EKF・UKFの比較

ステップ	カルマンフィルタ（KF）	拡張カルマンフィルタ（EKF）	アンセンテッドカルマンフィルタ（UKF）
モデル	線形のF、H、ガウスノイズ	非線形のf、h。推定値まわりで線形化	非線形のf、h。微分は不要
予測（平均）	x = F x + B u	x = f(x, u)	2n+1個のシグマ点をfに通し、重み付き平均をとる
予測（共分散）	P = F P F^T + Q	P = Fj P Fj^T + Q、Fj = xにおけるdf/dx	伝播した点の重み付き外積の和にQを加える
予測観測値	H x	h(x)	シグマ点にhを適用した値の重み付き平均
イノベーション	y = z - H x	y = z - h(x)	y = z - z_mean
イノベーション共分散	S = H P H^T + R	S = Hj P Hj^T + R、Hj = xにおけるdh/dx	hを通したシグマ点の重み付き広がりにRを加える
ゲイン	K = P H^T S^-1	K = P Hj^T S^-1	K = Pxz S^-1（Pxzは状態と観測の相互共分散）
更新	x = x + K y、P = (I - K H) P	同じ式で、HをHjに置き換える	x = x + K y、P = P - K S K^T
1ステップの計算量	行列積と、p x p行列の逆行列1回	KFの計算量に加えてヤコビ行列2つ	fとhをそれぞれ2n+1回呼び出し、行列平方根を1回
破綻する条件	モデルが線形でない	1標準偏差の範囲内で非線形性が強い、ヤコビ行列が誤っている	信念分布が強く非ガウス、または多峰性（粒子フィルタを使う）

共分散の更新はJoseph形式で書いてください。丸め誤差があってもPが対称かつ正定値に保たれます。

P = (I - K H) P (I - K H)^T + K R K^T

UKFのシグマ点

ほとんどのUKF実装で使われるスケーリング付きシグマ点（たとえばFilterPyのMerweScaledSigmaPoints）は次のとおりです。

lambda   = alpha^2 (n + kappa) - n
X_0      = x
X_i      = x + column_i( sqrt((n + lambda) P) )        i = 1..n
X_(n+i)  = x - column_i( sqrt((n + lambda) P) )
Wm_0     = lambda / (n + lambda)
Wc_0     = lambda / (n + lambda) + (1 - alpha^2 + beta)
Wm_i     = Wc_i = 1 / (2 (n + lambda))                 i = 1..2n
alpha small (FilterPy suggests 1e-3), beta = 2 for Gaussian priors, kappa often 0

UKFでは角度の扱いに注意が必要です。シグマ点の方位角は円周平均で平均し、すべての差を折り返してください。そうしないと、±π付近に散らばった値の平均がゼロになります。

QとRの選び方

行列	求め方	確認方法
R	ロボットを静止させてセンサのログをとり、チャネルごとの分散を求める。またはデータシートのノイズ密度とセンサの帯域幅から導く	実データでNISの平均がpに近い
Q	ノイズモデルから導く。標準偏差sigma_aの白色加速度で駆動される等速度モデルでは、G = (dt^2 / 2, dt)としてQ = G G^T sigma_a^2を使う。sigma_aはプラットフォームが実際に出せる加速度から選ぶ	シミュレーションでNEES、実データでNIS
P0	正直な初期不確かさ。小さすぎると、フィルタが初期の観測を無視する	最初の数ステップのNEES

QとRの比が挙動を決めます。Qが大きいと観測を信頼し、推定値のノイズが多くなります。Qが小さいとモデルを信頼し、モデルが誤っているときに遅れたり発散したりします。Q、R、P0を同じ倍率でスケーリングするとPは変わりますが、推定値は変わりません。

一貫性の確認

検定	統計量	必要なもの	一貫していると判断する条件
NEES	e^T P^-1 e、ただしe = 真値 - 推定値	真値（シミュレーション、モーションキャプチャ）	N回の試行で平均した値が、自由度N nのカイ二乗区間をNで割った範囲に入る
NIS	y^T S^-1 y	フィルタ自身のイノベーションだけなので、実データでも使える	平均値が、自由度N pのカイ二乗区間をNで割った範囲に入る
白色性	イノベーションの自己相関	記録したイノベーション	ゼロ以外のすべてのラグでほぼゼロ。イノベーションに相関があれば、状態の不足かモデルの誤りを示す

NEESとNISを計算する実行可能なカルマンフィルタ

等速度で動く目標の位置を10 Hzで観測します。このコードは、真のQで100回のモンテカルロ試行を行い、続いてQを100分の1にした場合と100倍にした場合も同様に試行し、NEESとNISの平均を95%区間と比べて出力します。

import numpy as np
from scipy.stats import chi2

rng = np.random.default_rng(42)
DT, STEPS, RUNS = 0.1, 100, 100
F = np.array([[1.0, DT], [0.0, 1.0]])                 # constant velocity: state (position, velocity)
H = np.array([[1.0, 0.0]])                            # we measure position only
SIGMA_A, SIGMA_Z = 0.5, 0.2                           # accel noise (m/s^2), sensor noise (m)
G = np.array([[0.5 * DT**2], [DT]])
Q_TRUE = G @ G.T * SIGMA_A**2                         # discrete white-noise acceleration model
R = np.array([[SIGMA_Z**2]])


def kf_step(x, P, z, Q):
    x = F @ x                                         # predict
    P = F @ P @ F.T + Q
    y = z - H @ x                                     # innovation
    S = H @ P @ H.T + R
    K = P @ H.T @ np.linalg.inv(S)
    x = x + K @ y                                     # update
    I_KH = np.eye(2) - K @ H
    P = I_KH @ P @ I_KH.T + K @ R @ K.T               # Joseph form keeps P symmetric
    return x, P, (y.T @ np.linalg.solve(S, y)).item()


def monte_carlo(q_scale: float):
    nees, nis, sq_err = np.zeros((RUNS, STEPS)), np.zeros((RUNS, STEPS)), np.zeros((RUNS, STEPS))
    for i in range(RUNS):
        truth = np.array([[0.0], [1.0]])
        x, P = np.array([[0.0], [0.0]]), np.diag([1.0, 1.0])
        for k in range(STEPS):
            truth = F @ truth + G * rng.normal(0, SIGMA_A)
            z = H @ truth + rng.normal(0, SIGMA_Z)
            x, P, nis[i, k] = kf_step(x, P, z, Q_TRUE * q_scale)
            e = truth - x
            nees[i, k] = (e.T @ np.linalg.solve(P, e)).item()
            sq_err[i, k] = e[0, 0] ** 2
    return nees.mean(axis=0), nis.mean(axis=0), np.sqrt(sq_err.mean())


nees_band = chi2.ppf([0.025, 0.975], 2 * RUNS) / RUNS   # run-averaged NEES, state dim 2
nis_band = chi2.ppf([0.025, 0.975], 1 * RUNS) / RUNS    # run-averaged NIS, measurement dim 1
print(f"95 % bands: NEES {np.round(nees_band, 2)}, NIS {np.round(nis_band, 2)}")
for q_scale in (0.01, 1.0, 100.0):
    a_nees, a_nis, rmse = monte_carlo(q_scale)
    in_nees = np.mean((a_nees[10:] >= nees_band[0]) & (a_nees[10:] <= nees_band[1]))   # skip 10 warm-up steps
    in_nis = np.mean((a_nis[10:] >= nis_band[0]) & (a_nis[10:] <= nis_band[1]))
    print(f"Q x {q_scale:6.2f}: mean NEES {a_nees.mean():6.2f} ({100 * in_nees:3.0f} % of steps in band), "
          f"mean NIS {a_nis.mean():5.2f} ({100 * in_nis:3.0f} %), position RMSE {rmse:.3f} m")

出力：

95 % bands: NEES [1.63 2.41], NIS [0.74 1.3 ]
Q x   0.01: mean NEES  68.65 (  0 % of steps in band), mean NIS  1.97 ( 28 %), position RMSE 0.201 m
Q x   1.00: mean NEES   2.02 ( 94 % of steps in band), mean NIS  1.02 ( 93 %), position RMSE 0.097 m
Q x 100.00: mean NEES   1.13 (  0 % of steps in band), mean NIS  0.81 ( 77 %), position RMSE 0.132 m

真のQでは、2状態のフィルタに対して平均NEESは2.02となり、期待値の2に近い値です。Qが小さすぎると、フィルタは過信の状態になります。NEESは約69に跳ね上がり、位置誤差は2倍になります。Qが大きすぎると、フィルタは自信過小の状態になります。NEESは区間を下回り、推定値のノイズが増えます。NISも同じ方向に変化しますが、変化はより緩やかです。そして実機で計算できるのはNISのほうです。

EKF：ヤコビ行列を確認し、角度を折り返す

EKFのバグの多くは、ヤコビ行列の誤りか、イノベーション内の角度の折り返し忘れです。このコードは距離・方位の観測モデルのヤコビ行列を中心差分と比較し、折り返しの問題を示します。

import numpy as np

LANDMARK = np.array([4.0, 3.0])                      # known landmark position (m)


def h(x: np.ndarray) -> np.ndarray:
    """Range and bearing from a robot pose (x, y, theta) to the landmark."""
    dx, dy = LANDMARK - x[:2]
    return np.array([np.hypot(dx, dy), np.arctan2(dy, dx) - x[2]])


def h_jacobian(x: np.ndarray) -> np.ndarray:
    dx, dy = LANDMARK - x[:2]
    q = dx**2 + dy**2
    r = np.sqrt(q)
    return np.array([[-dx / r, -dy / r, 0.0],
                     [dy / q, -dx / q, -1.0]])


def numeric_jacobian(f, x: np.ndarray, eps: float = 1e-6) -> np.ndarray:
    cols = []
    for i in range(len(x)):
        d = np.zeros_like(x)
        d[i] = eps
        cols.append((f(x + d) - f(x - d)) / (2 * eps))   # central difference
    return np.array(cols).T


def wrap(a: float) -> float:
    return (a + np.pi) % (2 * np.pi) - np.pi


x = np.array([1.0, 0.5, 0.3])
err = np.abs(h_jacobian(x) - numeric_jacobian(h, x)).max()
print(f"analytic vs numeric Jacobian, max difference: {err:.1e}")

z_pred, z_meas = np.array([5.0, 3.10]), np.array([5.1, -3.12])   # bearing near +pi and -pi
naive = z_meas - z_pred
fixed = np.array([naive[0], wrap(naive[1])])
print("innovation without wrapping:", np.round(naive, 3))
print("innovation with wrapping:   ", np.round(fixed, 3))

折り返しをしないと、-πをわずかに越えた方位の観測値と、+πのわずかに手前の予測値から、-6.22 radのイノベーションが得られます。実際の差は0.063 radです。角度の差は、イノベーションでも状態の更新でも、すべて折り返してください。

落とし穴

• S^-1 yだけが必要なときは、S^-1を作らずにnp.linalg.solve(S, y)を使ってください。
• 観測はタイムスタンプ順に処理してください。遅れて届いた観測にはバッファと再計算が必要です。古いデータで更新してはいけません。
• Qを大きな倍率で膨らませたときにしか一貫性が保たれないフィルタは、通常モデル化の誤りを抱えています。バイアス状態の欠落、座標系の誤り、度とラジアンの単位の不一致などです。
• 外れ値はNISでゲートしてください。y^T S^-1 yが自由度pのカイ二乗しきい値を超える観測は棄却します。
• 姿勢については、小さな誤差角を推定し、姿勢全体は四元数（クォータニオン）で保持します（誤差状態フィルタ）。回転の表現を参照してください。

さらに読む

• R. E. Kalman, “A new approach to linear filtering and prediction problems”, 「Journal of Basic Engineering」 82(1), 35-45, 1960, doi:10.1115/1.3662552.
• S. J. Julier and J. K. Uhlmann, “Unscented filtering and nonlinear estimation”, 「Proceedings of the IEEE」 92(3), 401-422, 2004, doi:10.1109/JPROC.2003.823141.
• 差動駆動・A*・カルマンフィルタのプロジェクトでは、シミュレーション上のロボットでこれと同様のフィルタを使っています。
• 推定値を使う制御ループについてはPIDチューニングを参照してください。