GPSの位置表示は、車が動いていると数メートル単位でフラフラ揺れます。それでもカーナビが地図上に滑らかな1本の軌跡を描けるのはなぜでしょうか。秘密は、ノイズだらけの観測を鵜呑みにせず、「車は急にワープしない」という運動の知識と組み合わせて、最もありそうな位置を毎瞬間ベイズ的に推定しているからです。この推定をおこなう代表的なアルゴリズムがカルマンフィルタです。
カルマンフィルタは1960年代にアポロ計画の航法に使われて以来、ロケットや人工衛星の姿勢制御、ロボットの自己位置推定（SLAM）、株価や経済指標の時系列フィルタリング、さらにはセンサーフュージョンまで、「ノイズの中から隠れた真の状態を取り出す」あらゆる場面で使われ続けています。一度仕組みを理解すると、拡張カルマンフィルタ（EKF）やパーティクルフィルタといった発展手法も同じ枠組みの拡張として見通せるようになります。
本記事の内容
- カルマンフィルタの直感的な理解と状態空間モデルの定義
- 予測ステップと更新ステップの2段サイクル、カルマンゲインの導出（省略なし）
- 1次元の位置推定と2次元の等速度追跡をPythonで実装し、結果を考察
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
カルマンフィルタとは
カルマンフィルタを一言で言うと、「予想」と「観測」を、それぞれの信頼度に応じて加重平均する装置です。
身近な例で考えてみましょう。あなたが暗い廊下を一定の歩幅で歩いているとします。目を閉じていても「だいたい今このあたりだろう」と予想できます。これがモデルによる予測です。一方、ときどき薄目を開けると壁の位置がぼんやり見えます。これが観測です。目を閉じての予想も、薄目で見た観測も、どちらも完全には正確ではありません。賢い人なら、両方を都合よく組み合わせて「たぶんここだ」と判断するはずです。
カルマンフィルタがやっているのは、まさにこの「予想と観測の折衷」を、数学的に最適な形でおこなうことです。しかも単なる足して2で割る平均ではなく、「予想がどれくらいあてになるか」と「観測がどれくらいあてになるか」を毎回測り、あてになる方を重く扱うという賢い重み付けをします。この重みこそが後で出てくるカルマンゲインです。
上の図がカルマンフィルタの全体像です。左の予測ステップでは、運動モデルを使って状態を1歩先へ進めます。このとき将来は不確かなので、推定のばらつき（不確かさ）は増えます。右の更新ステップでは、新しい観測を取り込んで推定を修正します。観測という情報が増えるので、不確かさは減ります。この2ステップを毎時刻くり返すだけ、というのがカルマンフィルタの正体です。
このサイクルをきちんと数式で動かすには、まず「状態」と「観測」を数学的に表現する器が必要です。それが次の状態空間モデルです。
状態空間モデルで問題を定式化する
カルマンフィルタを動かすには、対象を状態空間モデルという形に書き下す必要があります。これは「目に見えない真の状態が時間とともにどう変化するか」と「その状態がどうやって観測に現れるか」を、2本の式で表したものです。
上の図のように、上段に並ぶ円が隠れた状態 $\bm{x}_k$ です。これは直接は見えません。状態は行列 $\bm{F}$ によって次の時刻へ遷移し、その途中で予測しきれない揺らぎ（プロセスノイズ $\bm{Q}$）が加わります。下段の四角が観測 $\bm{z}_k$ で、状態を観測行列 $\bm{H}$ で写したものに観測ノイズ $\bm{R}$ が乗ったものです。私たちが手にできるのは、この下段のノイズ混じりの観測だけです。
状態方程式（システムの時間発展）
まず、状態が時間とともにどう変わるかを表す式が状態方程式です。式を出す前に意味を予告すると、これは「1ステップ前の状態に運動モデルを掛けて未来を予測し、予測しきれないぶんをノイズで吸収する」式です。
$$ \bm{x}_k = \bm{F}\bm{x}_{k-1} + \bm{w}_k, \quad \bm{w}_k \sim \mathcal{N}(\bm{0}, \bm{Q}) $$
ここで各記号の意味は次の通りです。$\bm{x}_k$ は時刻 $k$ の状態ベクトル（推定したい量、たとえば位置と速度をまとめたもの）です。$\bm{F}$ は状態遷移行列で、運動の法則を行列にしたものです。$\bm{w}_k$ はプロセスノイズで、モデルでは表しきれない外乱（風、路面の凹凸など）を表し、平均ゼロ・共分散 $\bm{Q}$ のガウス分布に従うと仮定します。$\bm{Q}$ が大きいほど「モデルを信じきれない」ことを意味します。
観測方程式（状態と観測の関係）
次に、見えない状態が観測にどう現れるかを表す式が観測方程式です。これは「真の状態の一部または変換を、ノイズ込みで測っている」という関係を表します。
$$ \bm{z}_k = \bm{H}\bm{x}_k + \bm{v}_k, \quad \bm{v}_k \sim \mathcal{N}(\bm{0}, \bm{R}) $$
$\bm{z}_k$ は時刻 $k$ の観測ベクトル（センサーの値）です。$\bm{H}$ は観測行列で、状態のどの成分がどう観測に現れるかを決めます。たとえば位置と速度を状態に持っていても、GPSは位置しか測れないなら $\bm{H}$ は位置だけを取り出す行列になります。$\bm{v}_k$ は観測ノイズで、共分散 $\bm{R}$ のガウス分布に従います。$\bm{R}$ が大きいほど「センサーは雑」という意味です。
ここで大事なのは、$\bm{Q}$ と $\bm{R}$ の比がフィルタの性格を決めるという点です。モデルを信じるか、観測を信じるか。そのバランスを取るために、次の節ではすべてをガウス分布として扱う前提を導入します。
ガウス前提と2段サイクル
カルマンフィルタが美しく閉じた式になるのは、すべての不確かさをガウス分布で表すと仮定するからです。状態の推定値は1点ではなく、「平均 $\hat{\bm{x}}$ と共分散 $\bm{P}$ を持つガウス分布」として持ち回ります。平均が「いちばんありそうな状態」、共分散が「その自信のなさ（不確かさ）」を表します。
ガウス分布にはとても都合のよい性質があります。ガウス分布を線形変換してもガウス分布のままであり、2つのガウス分布を掛け合わせても（正規化すれば）またガウス分布になるのです。予測は線形変換、観測の取り込みはベイズの定理による掛け算なので、この2つの性質のおかげで、推定はずっとガウス分布として閉じた形で計算できます。
上の図は、予測（青）と観測（赤）という2つのガウス分布を融合した結果（緑）を示しています。注目すべきは、緑の事後分布が青と赤の間に位置し、しかもどちらよりも背が高く幅が狭いことです。2つの独立な情報を合わせると、推定の不確かさはどちらか単独より必ず小さくなる。これがカルマンフィルタが「観測を取り込むほど賢くなる」理由を端的に表しています。
予測ステップ
予測ステップでは、1時刻前の事後推定 $(\hat{\bm{x}}_{k-1}, \bm{P}_{k-1})$ を状態方程式で進めます。事前推定をハット付きにマイナス記号で表すと、平均は単純に遷移行列を掛けるだけです。
$$ \hat{\bm{x}}_k^- = \bm{F}\hat{\bm{x}}_{k-1} $$
共分散の方は、まず遷移行列で変換され、さらにプロセスノイズの分だけ広がります。
$$ \bm{P}_k^- = \bm{F}\bm{P}_{k-1}\bm{F}^\top + \bm{Q} $$
$\bm{F}\bm{P}\bm{F}^\top$ という形は、ガウス分布を線形変換 $\bm{F}$ で写すと共分散が両側から $\bm{F}$ で挟まれるという、多変量ガウスの基本性質そのものです。そこに $\bm{Q}$ を足すのは「未来は予測しきれない分だけ不確かになる」ことを表します。予測ステップでは必ず不確かさが増える、と覚えておきましょう。
更新ステップ
更新ステップでは、新しい観測 $\bm{z}_k$ を取り込みます。まず、予測から期待される観測 $\bm{H}\hat{\bm{x}}_k^-$ と実際の観測とのズレ（イノベーション）を計算します。
$$ \bm{y}_k = \bm{z}_k – \bm{H}\hat{\bm{x}}_k^- $$
このズレをどれだけ信じて推定を直すかを決めるのがカルマンゲイン $\bm{K}_k$ です（導出は次節）。ゲインを使って平均と共分散を更新します。
$$ \hat{\bm{x}}_k = \hat{\bm{x}}_k^- + \bm{K}_k \bm{y}_k $$
$$ \bm{P}_k = (\bm{I} – \bm{K}_k\bm{H})\bm{P}_k^- $$
平均の更新式は「予測値に、ズレのゲイン倍を足して補正する」という形です。ゲインが大きいほど観測を強く信じ、ゲインが小さいほど予測（モデル）を信じます。更新ステップでは観測という情報が加わるので、共分散は必ず減ります。
では、その更新の重みである $\bm{K}_k$ はどう決めればよいのでしょうか。次節で「更新後の不確かさを最小にする」という基準から導きます。
カルマンゲインの導出
ここでのゴールは、更新後の推定誤差共分散 $\bm{P}_k$ のトレース（対角和、つまり全成分の分散の合計）を最小にするゲイン $\bm{K}_k$ を求めることです。トレースを最小にするとは、推定の平均二乗誤差を最小にすることに他なりません。
まず、更新後の共分散をゲインの一般形で書き下します。$\hat{\bm{x}}_k = \hat{\bm{x}}_k^- + \bm{K}(\bm{z}_k – \bm{H}\hat{\bm{x}}_k^-)$ から誤差 $\bm{e}_k = \bm{x}_k – \hat{\bm{x}}_k$ を計算し、その共分散をとると、任意の $\bm{K}$ に対して次のジョセフ形式が得られます。
$$ \bm{P}_k = (\bm{I} – \bm{K}\bm{H})\bm{P}_k^-(\bm{I} – \bm{K}\bm{H})^\top + \bm{K}\bm{R}\bm{K}^\top $$
この形を導くときに、予測誤差と観測ノイズが無相関であること、つまり交差項が消えることを使っています。次に、これを展開して $\bm{K}$ の式に整理します。
$$ \bm{P}_k = \bm{P}_k^- – \bm{K}\bm{H}\bm{P}_k^- – \bm{P}_k^-\bm{H}^\top\bm{K}^\top + \bm{K}(\bm{H}\bm{P}_k^-\bm{H}^\top + \bm{R})\bm{K}^\top $$
ここで観測のばらつきをまとめて $\bm{S} = \bm{H}\bm{P}_k^-\bm{H}^\top + \bm{R}$（イノベーション共分散）とおくと、最後の項が $\bm{K}\bm{S}\bm{K}^\top$ と簡潔になります。トレースを最小化したいので、$\mathrm{tr}(\bm{P}_k)$ を $\bm{K}$ で微分してゼロとおきます。行列の微分公式 $\frac{\partial}{\partial \bm{K}}\mathrm{tr}(\bm{K}\bm{A}) = \bm{A}^\top$ と $\frac{\partial}{\partial \bm{K}}\mathrm{tr}(\bm{K}\bm{B}\bm{K}^\top) = 2\bm{K}\bm{B}$（$\bm{B}$ は対称）を使うと、
$$ \frac{\partial\, \mathrm{tr}(\bm{P}_k)}{\partial \bm{K}} = -2(\bm{H}\bm{P}_k^-)^\top + 2\bm{K}\bm{S} = \bm{0} $$
両辺を整理して $\bm{K}$ について解くと、$\bm{K}\bm{S} = \bm{P}_k^-\bm{H}^\top$ となり、右から $\bm{S}^{-1}$ を掛けて次のカルマンゲインが得られます。
$$ \bm{K}_k = \bm{P}_k^-\bm{H}^\top\bm{S}^{-1} = \bm{P}_k^-\bm{H}^\top(\bm{H}\bm{P}_k^-\bm{H}^\top + \bm{R})^{-1} $$
この式の意味を読み解きましょう。分子の $\bm{P}_k^-\bm{H}^\top$ は「予測の不確かさ」を表し、分母の $\bm{S}$ は「予測の不確かさ＋観測の不確かさ」を表します。つまりゲインは「予測の不確かさ」を「全体の不確かさ」で割った比率です。予測が不確か（$\bm{P}^-$ が大きい）なら観測を信じてゲインは大きく、観測が雑（$\bm{R}$ が大きい）ならゲインは小さくなり予測を信じる。これがカルマンフィルタの賢さの核心です。
ここまでで理論は完結しました。次は実際に数値を入れて、本当に真値へ収束するのかを1次元の例で確かめます。
具体例：1次元の位置推定
いちばん簡単な例として、一定の位置にある対象をノイズ混じりのセンサーで何度も測る問題を考えます。状態は位置 $x$ のスカラー、状態は動かないので $F=1$、センサーは位置を直接測るので $H=1$ とします。プロセスノイズ $Q$ はごく小さく、観測ノイズ $R=4$（標準偏差2）とします。
この場合、すべてスカラーになるので予測・更新の式は次のように簡単になります。$P^- = P + Q$、$K = P^- / (P^- + R)$、$x \leftarrow x + K(z – x)$、$P \leftarrow (1-K)P^-$。初期値は位置を $\hat{x}_0 = 0$、不確かさを大きめに $P_0 = 100$ としておきます。本当の位置は20とします。
上の図を見ると、灰色の観測値は真値20のまわりで大きく散らばっているのに、青いカルマン推定値は数ステップで20付近へ吸い寄せられ、その後はほとんど動かず安定しています。青い帯（95%信頼区間）も急速に細くなり、推定が自信を深めていく様子が見て取れます。実際、最終ステップでの推定値は19.93、誤差共分散は0.085まで縮みました。
なぜ最初は大きく動き、後半は動かなくなるのでしょうか。その答えがカルマンゲインの推移にあります。
左の図のカルマンゲイン $K$ は、最初のステップでは0.96とほぼ1に近く、観測をほぼそのまま信じています。これは初期の不確かさ $P_0=100$ が非常に大きいためです。観測を重ねるごとにゲインは急速に下がり、やがて0.02程度で安定します。右の図の誤差共分散 $P$ も対数軸で一直線に減っていきます。十分に観測がたまると「もう新しい観測1個では推定はほとんど変わらない」状態になる、というのが直感的な意味です。
1次元では状態が1つだけでしたが、現実の追跡では位置と速度を同時に推定したくなります。次は状態がベクトルになる2次元の例に進みましょう。
2次元の等速度モデルによる追跡
移動する物体を追跡するとき、観測できるのは位置だけでも、速度まで一緒に推定したい場面はよくあります。カルマンフィルタの真価はここで発揮されます。状態ベクトルに位置と速度の両方を入れておけば、位置の観測だけから速度を「あぶり出す」ことができるのです。
状態ベクトルを $\bm{x} = [x, y, v_x, v_y]^\top$（位置2成分と速度2成分）とします。「速度が一定なら、次の位置は現在位置に速度×時間を足したもの」という等速度の運動法則を行列で書くと、状態遷移行列は次のようになります。
$$ \bm{F} = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}, \quad \bm{H} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \end{bmatrix} $$
観測行列 $\bm{H}$ は状態から位置成分 $x, y$ だけを取り出す形です。速度は観測には現れません。それでもカルマンフィルタは、位置の時間変化のパターンから速度を推定します。
上の図では、灰色の観測点（GPSのような位置観測）が真の軌跡（黒破線）のまわりで大きくバラついています。それに対して青いカルマン推定軌跡は、ノイズに振り回されずに滑らかな1本の線になっています。実際、観測そのものの位置誤差（RMSE）が約4.5だったのに対し、推定後は約2.2まで半減しました。運動モデルという「常識」を使うことで、生の観測より正確な軌跡が得られるのです。
この追跡で、各時刻の推定が「どれくらい確からしいか」を可視化したのが次の共分散楕円です。
共分散楕円による不確かさの可視化
2次元以上の推定では、不確かさは1つの数字ではなく共分散行列で表されます。位置の共分散行列 $\bm{P}_{xy}$ の固有ベクトルと固有値を使うと、不確かさを楕円として描けます。楕円が大きいほど自信がなく、小さいほど自信がある状態です。楕円の向きは、$x$ と $y$ の誤差の相関を表します。
上の図は、追跡開始から各時刻における位置の95%（2σ）共分散楕円を重ねたものです。$k=0$ では初期の不確かさが大きく楕円も大きいですが、$k=5, 10$ と観測を取り込むにつれて楕円はみるみる縮小し、$k=20$ 以降はほぼ一定の小さな楕円に落ち着きます。これは1次元の例で共分散 $P$ が減衰したのと同じ現象を、2次元で目に見える形にしたものです。カルマンフィルタが「確信を深めていく」過程が、楕円の収縮として直感的に理解できます。
楕円の収縮は「推定の質」を表していました。では、観測していない速度成分は実際どこまで復元できているのでしょうか。
観測しない速度の復元と、QとRの効き
カルマンフィルタの面白さは、観測に現れない量まで推定できる点にあります。2次元追跡では速度を一度も観測していませんが、位置の変化から速度を逆算しています。
左図は位置成分の推定で、観測（灰色）を平滑化しつつ真値（黒破線）を追えています。注目は右図で、一度も観測していない $x$ 方向の速度（緑）が、真の速度（黒破線）にきちんと追従しています。最初の数ステップは初期値の影響で外していますが、すぐに正しい速度へ収束します。位置という間接的な情報だけから速度という隠れた状態を復元する。これがセンサーフュージョンの基礎になる強力な性質です。
最後に、フィルタの性格を決める $\bm{Q}$ と $\bm{R}$ の効きを見ておきましょう。実務でカルマンフィルタを調整するとき、結局この2つの比をどう取るかがチューニングのほぼすべてです。
上の図は、途中で真値が10から16へジャンプする状況で、プロセスノイズ $Q$ を変えた2つのフィルタを比べたものです。$Q$ が小さい青は「モデルを強く信じる」ので軌跡は非常に滑らかですが、真値の変化への追従が遅れます。$Q$ が大きいオレンジは「観測を信じる」ので変化に素早く追従しますが、観測ノイズに振り回されてギザギザします。$Q$ を大きくするほど追従は速いが揺れやすい、小さくするほど滑らかだが鈍いというトレードオフが、はっきり読み取れます。$R$ を変えても同様の効果が逆向きに効きます。
Pythonでのスクラッチ実装
ここまでの議論を、汎用的なカルマンフィルタのクラスにまとめておきましょう。行列演算でそのまま書けるので、1次元でも多次元でも同じコードで動きます。
import numpy as np
class KalmanFilter:
def __init__(self, F, H, Q, R, x0, P0):
self.F, self.H, self.Q, self.R = F, H, Q, R
self.x, self.P = x0, P0
def predict(self):
# 予測ステップ: 状態を1歩進め、不確かさを広げる
self.x = self.F @ self.x
self.P = self.F @ self.P @ self.F.T + self.Q
return self.x
def update(self, z):
# 更新ステップ: 観測を取り込んで推定を修正する
y = z - self.H @ self.x # イノベーション
S = self.H @ self.P @ self.H.T + self.R # イノベーション共分散
K = self.P @ self.H.T @ np.linalg.inv(S) # カルマンゲイン
self.x = self.x + K @ y
I = np.eye(self.P.shape[0])
self.P = (I - K @ self.H) @ self.P
return self.x
このクラスは理論の式をそのまま写したものです。predict が予測ステップ、update が更新ステップに対応し、毎時刻この2つを順に呼ぶだけで状態推定が進みます。np.linalg.inv でイノベーション共分散の逆行列をとっている部分が、カルマンゲインの計算の中心です。設計上の判断として、状態を平均 x と共分散 P のペアで持ち回ることで、推定値だけでなく「その確からしさ」も同時に追跡できるようにしています。
次に、このクラスを2次元の等速度追跡に適用してみます。
import numpy as np
np.random.seed(21)
dt = 1.0
F = np.array([[1, 0, dt, 0], [0, 1, 0, dt],
[0, 0, 1, 0], [0, 0, 0, 1]], dtype=float)
H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], dtype=float)
G = np.array([[0.5 * dt**2, 0], [0, 0.5 * dt**2], [dt, 0], [0, dt]])
Q = G @ G.T * 0.02 # 加速度由来のプロセスノイズ
R = np.diag([9.0, 9.0]) # 観測ノイズ（標準偏差3）
# 真の軌跡（緩やかに曲がる等速度運動）と位置観測の生成
N = 50
xt = np.zeros((N, 4)); xt[0] = [0, 0, 1.0, 0.8]
for k in range(1, N):
acc = np.array([0.0, 0.0, 0.02 * np.sin(k * 0.2), -0.01])
xt[k] = F @ xt[k-1] + acc
meas = xt[:, :2] + np.random.normal(0, 3.0, (N, 2))
# カルマンフィルタの実行
kf = KalmanFilter(F, H, Q, R,
x0=np.zeros(4), P0=np.eye(4) * 50.0)
est = []
for k in range(N):
kf.predict()
est.append(kf.update(meas[k]).copy())
est = np.array(est)
rmse_meas = np.sqrt(np.mean(np.sum((meas - xt[:, :2])**2, axis=1)))
rmse_est = np.sqrt(np.mean(np.sum((est[:, :2] - xt[:, :2])**2, axis=1)))
print(f"観測RMSE={rmse_meas:.3f}, 推定RMSE={rmse_est:.3f}")
このコードを実行すると 観測RMSE=4.545, 推定RMSE=2.225 と出力されます。生の観測の位置誤差が約4.5だったのに対し、カルマンフィルタを通した推定の誤差は約2.2と半分以下になりました。運動モデルという事前知識を観測と組み合わせることで、観測そのものより正確な推定が得られる、という本記事の主張が数値でも裏付けられています。プロセスノイズ係数（コード中の0.02）を変えると、前節の $Q$ の効きのトレードオフを自分で確かめられます。
まとめ
本記事では、線形カルマンフィルタを直感から導出・実装まで通して解説しました。
- カルマンフィルタは、モデルによる予測とセンサーによる観測を、それぞれの不確かさに応じて加重平均する装置である
- 対象を状態方程式（$\bm{F}, \bm{Q}$）と観測方程式（$\bm{H}, \bm{R}$）からなる状態空間モデルで表し、すべての不確かさをガウス分布で扱う
- 予測ステップ（不確かさが増える）と更新ステップ（不確かさが減る）の2段サイクルをくり返す
- カルマンゲインは事後共分散のトレースを最小化する条件から導かれ、「予測の不確かさ÷全体の不確かさ」という比率として解釈できる
- 位置の観測だけから速度のような隠れた状態も推定でき、$\bm{Q}$ と $\bm{R}$ の比でフィルタの追従性と滑らかさを調整する
カルマンフィルタは「線形・ガウス」という前提のもとで最適なフィルタです。では、対象が非線形だったり、ノイズがガウスでなかったらどうするのか。その問いに答えるのが発展手法です。線形化で非線形に対応する拡張カルマンフィルタ（EKF）、シグマ点で非線形変換を近似する無香カルマンフィルタ（UKF）、そしてガウス前提すら捨てて多数の粒子で任意の分布を表すパーティクルフィルタへと、本記事の枠組みは自然に拡張されていきます。
次のステップとして、以下の記事も参考にしてください。