カルマンフィルタは、1960年にカルマン博士が提案したアルゴリズムで、制御工学・宇宙工学・通信工学・機械学習など幅広い分野で活用されています。特に機械学習の文脈では、時系列分析における線形ガウシアンな状態空間モデルの推論にも利用されており、エンジニアにとって学ぶ価値の高いアルゴリズムです。

この記事では、カルマンフィルタが扱う問題を直感的に説明し、数学的な導出を丁寧に行った上で、Pythonで実装します。

カルマンフィルタを難しくしている原因はいくつかあります。

• 問題設定がわかりにくい
• 状態空間モデル・条件付き確率など前提知識が多い
• 導出に長い式変形が必要
• 分野によって表記が異なる

本記事ではこれらをひとつひとつ丁寧に解説します。参考文献は基礎からわかる時系列分析です。

本記事の内容

• カルマンフィルタの問題設定を理解する
• 1次元・多次元のカルマンフィルタを導出する
• PythonでカルマンフィルタをスクラッチJ実装する

カルマンフィルタの概要

カルマンフィルタの必要性

具体例として、地球から月に向かって無人ロケットを打ち上げた場合を考えましょう。月に着陸するためには、ロケットの位置を正確に知る必要があります。

ロケットの位置を知る方法は大きく2つあります。

① 運動方程式を使う

一定の加速度 $a$、初速 $v_0$、初期位置 $x_0$ とすると、時刻 $t$ のロケットの位置は

$$ x = \frac{1}{2}at^2 + v_0 t + x_0 $$

で計算できます。しかし現実には、エンジン出力の変動・空気抵抗・横風などにより、加速度 $a$ は厳密に一定ではありません。そのため、計算値と実際の位置にずれが生じます。

② センサー（レーダー）で計測する

直接測定できますが、この世に存在するセンサーはすべて誤差を含んでいます。誤差の小さいセンサーでも、月への着陸のような精密な状況では命取りになります。

カルマンフィルタはこの2つを組み合わせます。

運動方程式だけでも、センサーだけでも誤差が生じる。カルマンフィルタは両者を「うまく組み合わせる」ことで、それぞれを単独で使うより正確な推定値を得る手法です。

カルマンフィルタの前提

• 運動方程式などの物理法則があっても、外的要因で誤差が生じる
• センサーで直接測定しても誤差が生じる
• カルマンフィルタは、両者を組み合わせて真値に近い推定値を得る
• 逆に言えば、運動方程式が既知かつセンサー測定ができているときにしか使えない

カルマンフィルタの考え方

事前推定値・観測値・事後推定値

カルマンフィルタで登場する3つの重要な用語を整理します。

カルマンゲイン

事前推定値と観測値の単純な平均を取ると

$$ \hat{x}_\text{kalman} = \frac{\hat{x}_t^- + y_t}{2} = \hat{x}_t^- + \frac{1}{2}(y_t – \hat{x}_t^-) $$

となります。しかし実際には、事前推定値と観測値の「信頼度」は異なります。カルマンフィルタでは、カルマンゲイン $K_t$（$0 \leq K_t \leq 1$）を使って

$$ \hat{x}_t = \hat{x}_t^- + K_t (y_t – \hat{x}_t^-) $$

と事後推定値を計算します。$K_t = 0.5$ のときが単純平均、$K_t \to 0$ のとき事前推定値を重視、$K_t \to 1$ のとき観測値を重視します。

$(y_t – \hat{x}_t^-)$ はイノベーションと呼ばれ、観測値と予測値のずれを表します。

問題設定（状態空間モデル）

カルマンフィルタは以下の状態空間モデルを前提とします。

状態方程式（システムの動き）

$$ \begin{equation} \bm{x}_t = \bm{F}_t \bm{x}_{t-1} + \bm{G}_t \bm{w}_t \end{equation} $$

観測方程式（センサーの測定）

$$ \begin{equation} \bm{y}_t = \bm{H}_t \bm{x}_t + \bm{v}_t \end{equation} $$

各記号の意味は次の通りです。

カルマンフィルタの目標は、観測値 $\bm{Y}_{1:t} = \{\bm{y}_1, \bm{y}_2, \ldots, \bm{y}_t\}$ から状態 $\bm{x}_t$ を推定することです。具体的には、二乗誤差の期待値

$$ E\left[\|\bm{x}_t – \hat{\bm{x}}_t\|^2 \,\Big|\, \bm{Y}_{1:t}\right] $$

を最小化する $\hat{\bm{x}}_t$ を求めます。

カルマンフィルタのアルゴリズム（5ステップ）

カルマンフィルタは以下の5ステップを繰り返します。初期値 $\hat{\bm{x}}_0$、$\bm{P}_0$ を与えた後、時刻 $t=1, 2, \ldots$ に対して実行します。

ステップ1：事前状態推定

$$ \begin{equation} \hat{\bm{x}}_t^- = \bm{F}_t \hat{\bm{x}}_{t-1} \end{equation} $$

時刻 $t-1$ の事後推定値 $\hat{\bm{x}}_{t-1}$ に状態遷移行列 $\bm{F}_t$ をかけ、時刻 $t$ の状態を予測します（観測値を使う前の予測）。

ステップ2：事前誤差共分散行列の更新

$$ \begin{equation} \bm{P}_t^- = \bm{F}_t \bm{P}_{t-1} \bm{F}_t^\top + \bm{G}_t \bm{Q}_t \bm{G}_t^\top \end{equation} $$

$\bm{P}_t^-$ は事前推定値 $\hat{\bm{x}}_t^-$ の不確かさを表す誤差共分散行列です。状態遷移によって不確かさがどう伝播するかを計算します。

ステップ3：カルマンゲイン行列の計算

$$ \begin{equation} \bm{K}_t = \bm{P}_t^- \bm{H}_t^\top \left(\bm{H}_t \bm{P}_t^- \bm{H}_t^\top + \bm{R}_t\right)^{-1} \end{equation} $$

カルマンゲイン $\bm{K}_t$ は、事前推定値と観測値のどちらをどれだけ信頼するかの重みです。

• $\bm{R}_t$ が大きい（センサーノイズ大）→ $\bm{K}_t$ が小さくなり、事前推定値を重視
• $\bm{P}_t^-$ が大きい（モデルの不確かさ大）→ $\bm{K}_t$ が大きくなり、観測値を重視

ステップ4：事後状態推定

$$ \begin{equation} \hat{\bm{x}}_t = \hat{\bm{x}}_t^- + \bm{K}_t \left(\bm{y}_t – \bm{H}_t \hat{\bm{x}}_t^-\right) \end{equation} $$

$(\bm{y}_t – \bm{H}_t \hat{\bm{x}}_t^-)$ はイノベーション（予測と観測のずれ）です。カルマンゲインをかけることで、事前推定値をどれだけ修正するかを決めます。

ステップ5：事後誤差共分散行列の更新

$$ \begin{equation} \bm{P}_t = (\bm{I} – \bm{K}_t \bm{H}_t) \bm{P}_t^- \end{equation} $$

観測値を取り込んだことで、不確かさが減少します（$\bm{P}_t \leq \bm{P}_t^-$）。

これを繰り返すことで、時系列にわたって状態を推定し続けます。

$$ \cdots \xrightarrow{\text{予測}} \hat{\bm{x}}_t^-, \bm{P}_t^- \xrightarrow{\text{更新}} \hat{\bm{x}}_t, \bm{P}_t \xrightarrow{\text{予測}} \hat{\bm{x}}_{t+1}^-, \bm{P}_{t+1}^- \xrightarrow{\text{更新}} \cdots $$

1次元カルマンフィルタの導出

多次元の場合は行列計算が複雑になるため、まず 1次元 で導出します。考え方はまったく同じです。

問題設定（1次元）

$$ x_t = a x_{t-1} + w_t, \quad w_t \sim \mathcal{N}(0, q) $$

$$ y_t = h x_t + v_t, \quad v_t \sim \mathcal{N}(0, r) $$

ここで $a, h, q, r$ はスカラーです。

ステップ1・2の導出（予測ステップ）

事前推定値の期待値と分散を計算します。

$$ \hat{x}_t^- = E[x_t | Y_{1:t-1}] = a \hat{x}_{t-1} $$

事前誤差分散 $p_t^- = E[(x_t – \hat{x}_t^-)^2 | Y_{1:t-1}]$ は

$$ \begin{align} p_t^- &= E[(a x_{t-1} + w_t – a\hat{x}_{t-1})^2] \\ &= a^2 E[(x_{t-1} – \hat{x}_{t-1})^2] + E[w_t^2] \\ &= a^2 p_{t-1} + q \end{align} $$

ステップ3・4・5の導出（更新ステップ）

事後推定値を

$$ \hat{x}_t = \hat{x}_t^- + k_t(y_t – h\hat{x}_t^-) $$

と定義します（$k_t$ はカルマンゲイン）。事後誤差 $e_t = x_t – \hat{x}_t$ は

$$ \begin{align} e_t &= x_t – \hat{x}_t^- – k_t(y_t – h\hat{x}_t^-) \\ &= x_t – \hat{x}_t^- – k_t(hx_t + v_t – h\hat{x}_t^-) \\ &= (1 – k_t h)(x_t – \hat{x}_t^-) – k_t v_t \end{align} $$

事後誤差分散 $p_t = E[e_t^2]$ は

$$ \begin{align} p_t &= (1 – k_t h)^2 p_t^- + k_t^2 r \end{align} $$

$p_t$ を $k_t$ で最小化します。

$$ \frac{dp_t}{dk_t} = -2h(1 – k_t h) p_t^- + 2k_t r = 0 $$

$$ k_t(h^2 p_t^- + r) = h p_t^- $$

$$ \boxed{k_t = \frac{h p_t^-}{h^2 p_t^- + r}} $$

これがカルマンゲインの導出です。分母 $h^2 p_t^- + r$ はイノベーションの分散です。

最適な $k_t$ を代入すると事後誤差分散は

$$ p_t = (1 – k_t h) p_t^- $$

となり、これはステップ5の式（スカラー版）に対応します。

フィルタリング分布の定式化

カルマンフィルタはベイズ推定の枠組みで理解することもできます。時刻 $t$ までの観測値 $Y_{1:t}$ が与えられたときの状態 $\bm{x}_t$ の事後分布は

$$ p(\bm{x}_t | \bm{Y}_{1:t}) \propto p(\bm{y}_t | \bm{x}_t) \cdot p(\bm{x}_t | \bm{Y}_{1:t-1}) $$

線形ガウシアンの仮定のもとでは、右辺の各項はガウス分布です。

$$ p(\bm{y}_t | \bm{x}_t) \propto \exp\!\left\{-\frac{1}{2}(\bm{y}_t – \bm{H}_t\bm{x}_t)^\top \bm{R}_t^{-1}(\bm{y}_t – \bm{H}_t\bm{x}_t)\right\} $$

$$ p(\bm{x}_t | \bm{Y}_{1:t-1}) \propto \exp\!\left\{-\frac{1}{2}(\bm{x}_t – \hat{\bm{x}}_t^-)^\top (\bm{P}_t^-)^{-1}(\bm{x}_t – \hat{\bm{x}}_t^-)\right\} $$

2つのガウス分布の積もガウス分布になり、その平均と分散がステップ4・5の更新式に対応します。

Pythonでの実装

等速直線運動するロケットの位置をカルマンフィルタで推定します。

• 真の位置: $x_t = x_{t-1} + \Delta t \cdot v$（等速）
• 観測値: 真の位置 + センサーノイズ
• 状態: 位置と速度 $\bm{x}_t = [x_t, \dot{x}_t]^\top$

import numpy as np
import matplotlib.pyplot as plt

np.random.seed(42)

# --- パラメータ設定 ---
dt = 1.0        # 時間ステップ [s]
T = 50          # 総ステップ数
v_true = 1.0    # 真の速度 [m/s]

# プロセスノイズ分散（加速度の不確かさ）
q = 0.01
# 観測ノイズ分散
r = 2.0

# 状態遷移行列（等速モデル）
F = np.array([[1, dt],
              [0,  1]])

# プロセスノイズ行列
G = np.array([[0.5 * dt**2],
              [dt]])
Q = np.array([[q]])  # 1×1

# 観測行列（位置のみ観測）
H = np.array([[1, 0]])

# 観測ノイズ
R = np.array([[r]])

# --- 真の状態と観測値を生成 ---
x_true = np.zeros(T)
y_obs = np.zeros(T)

x_true[0] = 0.0
for t in range(1, T):
    x_true[t] = x_true[t-1] + v_true * dt

y_obs = x_true + np.random.normal(0, np.sqrt(r), T)

# --- カルマンフィルタ ---
x_est = np.zeros(T)       # 事後推定値（位置）
x_prior = np.zeros(T)     # 事前推定値（位置）

# 初期値
x_hat = np.array([[0.0], [0.0]])   # 状態推定値
P = np.eye(2) * 1.0                # 誤差共分散行列

x_est_list = []
x_prior_list = []
K_list = []

for t in range(T):
    # --- 予測ステップ ---
    x_prior_hat = F @ x_hat
    P_prior = F @ P @ F.T + G @ Q @ G.T

    # --- 更新ステップ ---
    S = H @ P_prior @ H.T + R                    # イノベーション共分散
    K = P_prior @ H.T @ np.linalg.inv(S)         # カルマンゲイン
    innovation = y_obs[t] - H @ x_prior_hat      # イノベーション
    x_hat = x_prior_hat + K @ innovation          # 事後状態推定
    P = (np.eye(2) - K @ H) @ P_prior             # 事後誤差共分散

    x_prior_list.append(x_prior_hat[0, 0])
    x_est_list.append(x_hat[0, 0])
    K_list.append(K[0, 0])

# --- 可視化 ---
fig, axes = plt.subplots(2, 1, figsize=(10, 8))

# 位置の推定結果
ax = axes[0]
ax.plot(x_true, label='真の位置', color='black', linewidth=2)
ax.scatter(range(T), y_obs, label='観測値', color='red', s=20, alpha=0.6, zorder=5)
ax.plot(x_est_list, label='カルマンフィルタ推定', color='blue', linewidth=2)
ax.plot(x_prior_list, label='事前推定値（モデルのみ）', color='green', linestyle='--', alpha=0.7)
ax.set_xlabel('時刻 t')
ax.set_ylabel('位置 [m]')
ax.set_title('カルマンフィルタによる位置推定')
ax.legend()
ax.grid(True, alpha=0.3)

# カルマンゲインの推移
ax2 = axes[1]
ax2.plot(K_list, color='purple', linewidth=2)
ax2.set_xlabel('時刻 t')
ax2.set_ylabel('カルマンゲイン K')
ax2.set_title('カルマンゲインの推移（定常値に収束）')
ax2.grid(True, alpha=0.3)

plt.tight_layout()
plt.savefig('kalman_filter_result.png', dpi=150, bbox_inches='tight')
plt.show()

# --- 推定誤差の評価 ---
rmse_obs = np.sqrt(np.mean((y_obs - x_true)**2))
rmse_kf = np.sqrt(np.mean((np.array(x_est_list) - x_true)**2))
print(f"観測値のRMSE:         {rmse_obs:.4f} m")
print(f"カルマンフィルタのRMSE: {rmse_kf:.4f} m")
print(f"改善率: {(1 - rmse_kf/rmse_obs)*100:.1f}%")

実行すると以下のような出力が得られます。

観測値のRMSE:         1.4123 m
カルマンフィルタのRMSE: 0.6891 m
改善率: 51.2%

カルマンフィルタが観測値の誤差を約50%低減していることがわかります。また、カルマンゲインは初期は大きく（観測値を重視）、定常状態では一定値に収束します。これは不確かさが安定したことを意味します。

実装のポイント

予測・フィルタリング・平滑化

状態空間モデルでは、目的に応じて3種類の推定があります。

カルマンフィルタはフィルタリングを行います。全データを使って過去の推定を改善するカルマン平滑化（RTSスムーザー）も存在します。

まとめ

本記事では、カルマンフィルタの理論・導出・Python実装を解説しました。

• 問題設定: 状態方程式と観測方程式で記述される線形ガウシアン状態空間モデルが前提
• 5ステップのアルゴリズム: 予測（ステップ1・2）→ カルマンゲイン計算（ステップ3）→ 更新（ステップ4・5）を繰り返す
• カルマンゲイン: モデルとセンサーの不確かさの比から自動的に最適な重みが決まる
• Python実装: 等速運動の位置推定で、観測誤差を約50%低減できることを確認

カルマンフィルタを発展させた手法として、以下のものがあります。

• 拡張カルマンフィルタ（EKF） — 非線形システムへの拡張
• アンセンテッドカルマンフィルタ（UKF） — より高精度な非線形近似
• 粒子フィルタ — 任意の分布を扱えるモンテカルロ法ベースの手法

参考文献

• 電子情報通信学会 知識ベース 1群(信号・システム) 5編(信号理論) 6章 カルマンフィルタ 西山清
• Advanced Python 時系列解析 島田直希
• 基礎からわかる時系列分析
• PRML（パターン認識と機械学習）