世界一分かりやすいカルマンフィルタの理論と導出と実装

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

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

まずは全体像です。カルマンフィルタは、図のように「運動方程式によるモデル予測」と「センサーによる観測」という2つの情報源を組み合わせます。どちらも単独では誤差を含みますが、うまく統合することで、どちらよりも正確な推定値を引き出せます。この記事を通じて、その「うまく統合する」中身を解き明かしていきます。

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

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

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

本記事の内容

• カルマンフィルタの問題設定を理解する
• 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$ のとき観測値を重視します。

カルマンゲイン $K_t$ は、予測と観測のどちらをどれだけ信じるかを決める「つまみ」だと考えてください。図のように、$K_t=0$ なら予測そのまま、$K_t=1$ なら観測そのまま、$K_t=0.5$ で両者の単純平均になります。実際の $K_t$ は、予測と観測それぞれの信頼度（分散）から自動的に決まります。その計算方法を、このあと導出していきます。

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

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

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

状態空間モデルは、カルマンフィルタが世界をどう捉えているかを表します。図のように、真の状態 $\bm{x}_t$（位置や速度）は状態遷移行列 $\bm{F}$ にしたがって時間発展し、観測 $\bm{y}_t$ はその状態にノイズが乗ったものとして得られます。私たちが実際に見られるのは下段の $\bm{y}_t$ だけで、隠れている上段の $\bm{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$ に対して実行します。

5つのステップは、図のように「予測」と「更新」の2つの局面に分かれます。予測（ステップ1・2）でモデルを使って先読みし、更新（ステップ3・4・5）で観測を取り込んで補正します。この予測→更新の往復をひたすら繰り返すことで、時系列にわたって状態を追い続けるのがカルマンフィルタの基本動作です。では各ステップを順に見ていきましょう。

ステップ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$ が大きくなり、観測値を重視

言い換えると、カルマンゲインは「予測の不確かさ $\bm{P}_t^-$」と「観測の不確かさ $\bm{R}_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^-$）。

この「不確かさ」の動きを図で追うと、カルマンフィルタの呼吸が見えてきます。予測ステップでは不確かさが増え（緑の $P^-$）、更新ステップで観測を取り込むと減ります（青の $P$）。この「増えては減る」をくり返しながら、全体として誤差を小さく保ち続けるのです。

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

$$ \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の式（スカラー版）に対応します。

数値で動きを体感する

式だけでは実感が湧きにくいので、1次元に具体的な数字を入れてみましょう。観測行列 $h=1$、観測ノイズ分散 $r=1$ とし、ある時刻の事前誤差分散が $p_t^-=4$（予測がかなり不確か）だったとします。

カルマンゲインは

$$ k_t = \frac{h\, p_t^-}{h^2 p_t^- + r} = \frac{4}{4 \times 1 + 1} = 0.8 $$

となります。予測の不確かさ（4）が観測ノイズ（1）より大きいので、$k_t = 0.8$ と観測を強く信頼する重みになりました。このとき事後誤差分散は

$$ p_t = (1 – k_t h)\, p_t^- = (1 – 0.8) \times 4 = 0.8 $$

と、観測を取り込む前の $4$ から $0.8$ へ大きく縮みます。「予測が不確かなときは観測を信じ、観測を取り込むと不確かさが一気に減る」——この呼吸が数字でも見えてきます。

逆に、予測がすでに正確で $p_t^- = 0.1$ だったなら $k_t = 0.1 / (0.1 + 1) \approx 0.09$ と小さくなり、観測をほとんど信じずに予測を維持します。カルマンゲインが「予測と観測の信頼度の比」で自動的に決まることが、具体的な数字からも納得できるはずです。

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

カルマンフィルタはベイズ推定の枠組みで理解することもできます。時刻 $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の更新式に対応します。

更新ステップで起きていることを確率分布で描くと、図のようになります。予測の分布（緑）と観測の分布（オレンジ）を掛け合わせると、事後分布（青）は両者の間に位置し、しかも分散が両者のどちらよりも小さくなります。「2つの不確かな情報を合わせると、より確かになる」——これがカルマンフィルタの核心を表す一枚です。

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.3455 m
カルマンフィルタのRMSE: 0.7450 m
改善率: 44.6%

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

結果を図で見てみましょう。オレンジの観測値は真値の周りで大きくばらついていますが、青のカルマンフィルタ推定は黒の真値にぴったり追従しています。緑の事前推定（モデルだけの予測）もそこそこ良いものの、観測を取り込んだカルマンフィルタのほうがより安定しているのがわかります。

カルマンゲインの動きにも注目してください。最初は大きな値（観測を重視して一気に真値へ寄せる）から始まり、時間とともに一定値へ収束していきます。これは推定の不確かさが落ち着いたことを意味し、定常カルマンフィルタと呼ばれる状態です。

精度を数値で比べると一目瞭然です。観測値だけのRMSEに対して、カルマンフィルタは誤差を約45%も減らせています。ノイズの多い同じ観測から、モデルの知識を組み合わせるだけでここまで精度が上がるのです。

実装のポイント

よくあるつまずきと考え方

カルマンフィルタでつまずきやすいポイントを、Q&A形式で整理しておきます。

Q. 「事前」と「事後」は何が違うの？ 事前推定値 $\hat{\bm{x}}_t^-$ は「観測 $\bm{y}_t$ を見る前に、モデルだけで予測した値」、事後推定値 $\hat{\bm{x}}_t$ は「観測を見た後に補正した値」です。予測ステップで事前を作り、更新ステップで事後に直す——この順番が5ステップの骨格になっています。記号の右肩の「$-$」が「まだ観測を見ていない」目印だと覚えておくと混乱しません。

Q. 誤差共分散 $\bm{P}$ は結局何を表しているの？ $\bm{P}$ は推定値の「自信のなさ（不確かさ）」を表します。対角成分が各状態量の分散で、大きいほど「その推定はあてにならない」という意味です。予測すると不確かさは増え、観測を取り込むと減る——この $\bm{P}$ の増減こそ、カルマンフィルタが自分の信頼度を管理している証拠です。

Q. なぜ位置だけでなく速度も状態に入れるの？ 位置だけだと「次にどこへ動くか」をモデルが予測できません。速度を状態に含めれば、等速・等加速度といった運動法則を状態遷移行列 $\bm{F}$ に書き込めるので、予測の精度が上がります。観測できるのが位置だけでも、速度は「予測と観測のつじつま合わせ」から自動的に推定されていきます。

Q. プロセスノイズ $\bm{Q}$ と観測ノイズ $\bm{R}$ はどう決めるの？ $\bm{R}$ はセンサーの仕様（測定誤差の分散）から見積もれることが多いです。$\bm{Q}$ はモデルの「信用できなさ」を表し、実データでの推定誤差を見ながら調整します。$\bm{Q}$ を大きくするとモデルを疑って観測重視に、小さくするとモデルを信じて滑らかな推定になります。この2つの比が、追従性と滑らかさのバランスを決める最大のツマミです。

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

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

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

まとめ

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

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

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

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

参考文献

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