Kalmanフィルタの計算手順
アルゴリズム概要
Kalmanフィルタが対象とするのは、線形状態空間モデルで記述される離散時間システムである：
\begin{align} \boldsymbol{x}_k &= \boldsymbol{F}_k \boldsymbol{x}_{k-1} + \boldsymbol{w}_k, \quad \boldsymbol{w}_k \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{Q}_k) \\ \boldsymbol{y}_k &= \boldsymbol{H}_k \boldsymbol{x}_k + \boldsymbol{v}_k, \quad \boldsymbol{v}_k \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{R}_k) \end{align}$\boldsymbol{x}_k$ は観測できない真の状態、$\boldsymbol{y}_k$ は観測値、$\boldsymbol{w}_k$ と $\boldsymbol{v}_k$ はそれぞれプロセスノイズと観測ノイズで、互いに独立なガウス分布と仮定する。$\boldsymbol{F}_k, \boldsymbol{H}_k$ は既知の行列。Kalmanフィルタの目的は、これまでの観測列 $\boldsymbol{y}_{1:k}$ から $\boldsymbol{x}_k$ の最良推定値 $\hat{\boldsymbol{x}}_{k|k}$ を得ることである。
この推定は 予測（Predict） と 更新（Update） の 2 ステップを交互に繰り返すことで逐次的に行える。
予測ステップ（Predict）
前回の推定値から、現在の状態と共分散を予測する。
- 状態予測: $\hat{\boldsymbol{x}}_{k|k-1} = \boldsymbol{F}_k \hat{\boldsymbol{x}}_{k-1|k-1}$
- 共分散予測: $\boldsymbol{P}_{k|k-1} = \boldsymbol{F}_k \boldsymbol{P}_{k-1|k-1} \boldsymbol{F}_k^T + \boldsymbol{Q}_k$
更新ステップ（Update）
観測値を使って、予測を補正する。
- 残差共分散: $\boldsymbol{S}_k = \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T + \boldsymbol{R}_k$
- Kalmanゲイン: $\boldsymbol{K}_k = \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T \boldsymbol{S}_k^{-1}$
- 残差: $\boldsymbol{e}_k = \boldsymbol{y}_k - \boldsymbol{H}_k \hat{\boldsymbol{x}}_{k|k-1}$
- 状態更新: $\hat{\boldsymbol{x}}_{k|k} = \hat{\boldsymbol{x}}_{k|k-1} + \boldsymbol{K}_k \boldsymbol{e}_k$
- 共分散更新: $\boldsymbol{P}_{k|k} = (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k) \boldsymbol{P}_{k|k-1}$
各式の導出については導出ページを参照。
擬似コード
Kalmanフィルタの計算手順を擬似コードで書くと次のようになる （太字の小文字は列ベクトル、太字の大文字は行列である）。
// 初期値 (時刻 0 の事前推定)
$\hat{\boldsymbol{x}}_{0|0} = $ (初期状態推定値);
$\boldsymbol{P}_{0|0} = $ (初期誤差共分散行列);
// 予測と推定を繰り返す
for (k = 1, 2, 3, ...) {
| // 予測ステップ（Predict） | ||
| $\hat{\boldsymbol{x}}_{k|k-1} = \boldsymbol{F}_k \hat{\boldsymbol{x}}_{k-1|k-1}$; | // 状態の予測 | |
| $\boldsymbol{P}_{k|k-1} = \boldsymbol{F}_k \boldsymbol{P}_{k-1|k-1} \boldsymbol{F}_k^T + \boldsymbol{Q}_k$; | // 予測誤差共分散 | |
| // 更新ステップ（Update） | ||
| $\boldsymbol{S}_k = \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T + \boldsymbol{R}_k$; | // 残差共分散 | |
| $\boldsymbol{K}_k = \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T \boldsymbol{S}_k^{-1}$; | // Kalmanゲイン | |
| $\boldsymbol{e}_k = \boldsymbol{y}_k - \boldsymbol{H}_k \hat{\boldsymbol{x}}_{k|k-1}$; | // 残差（イノベーション） | |
| $\hat{\boldsymbol{x}}_{k|k} = \hat{\boldsymbol{x}}_{k|k-1} + \boldsymbol{K}_k \boldsymbol{e}_k$; | // 状態の推定 | |
| $\boldsymbol{P}_{k|k} = (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k) \boldsymbol{P}_{k|k-1}$; | // 推定誤差共分散 | |
}
各変数の意味
| 変数 | 意味 | サイズ |
|---|---|---|
| $\boldsymbol{x}_k$ | 時刻 $k$ の真の状態（未知） | $n \times 1$ |
| $\boldsymbol{y}_k$ | 時刻 $k$ の観測値（観測可能） | $m \times 1$ |
| $\hat{\boldsymbol{x}}_{k|k-1}$ | 時刻 $k-1$ までの情報による時刻 $k$ の状態予測 | $n \times 1$ |
| $\hat{\boldsymbol{x}}_{k|k}$ | 時刻 $k$ までの情報による時刻 $k$ の状態推定 | $n \times 1$ |
| $\boldsymbol{P}_{k|k-1}$ | 予測誤差の分散共分散行列 | $n \times n$ |
| $\boldsymbol{P}_{k|k}$ | 推定誤差の分散共分散行列 | $n \times n$ |
| $\boldsymbol{K}_k$ | Kalmanゲイン（推定誤差を最小にする最適ゲイン） | $n \times m$ |
| $\boldsymbol{e}_k$ | 残差（イノベーション）：観測値と予測観測値の差 | $m \times 1$ |
| $\boldsymbol{S}_k$ | 残差の分散共分散行列 | $m \times m$ |
| $\boldsymbol{F}_k$ | 状態遷移行列 | $n \times n$ |
| $\boldsymbol{H}_k$ | 観測行列 | $m \times n$ |
| $\boldsymbol{Q}_k$ | プロセスノイズ $\boldsymbol{w}_k$ の分散共分散行列 | $n \times n$ |
| $\boldsymbol{R}_k$ | 観測ノイズ $\boldsymbol{v}_k$ の分散共分散行列 | $m \times m$ |
ここで $n$ は状態ベクトルの次元、$m$ は観測ベクトルの次元である。
予測ステップの詳細
予測ステップでは、前時刻の推定値から現時刻の状態を予測する。
状態予測
\begin{equation} \hat{\boldsymbol{x}}_{k|k-1} = \boldsymbol{F}_k \hat{\boldsymbol{x}}_{k-1|k-1} \end{equation}状態遷移行列 $\boldsymbol{F}_k$ を使って、前時刻の推定値を現時刻に射影する。 制御入力 $\boldsymbol{u}_k$ がある場合は：
\begin{equation} \hat{\boldsymbol{x}}_{k|k-1} = \boldsymbol{F}_k \hat{\boldsymbol{x}}_{k-1|k-1} + \boldsymbol{B}_k \boldsymbol{u}_k \end{equation}共分散予測
\begin{equation} \boldsymbol{P}_{k|k-1} = \boldsymbol{F}_k \boldsymbol{P}_{k-1|k-1} \boldsymbol{F}_k^T + \boldsymbol{Q}_k \end{equation}状態遷移により誤差共分散も伝播する。第 1 項は「既存の不確かさ $\boldsymbol{P}_{k-1|k-1}$ が $\boldsymbol{F}_k$ で変換される」部分、第 2 項はプロセスノイズで新たに加わる不確かさである。
$\boldsymbol{F}_k \boldsymbol{P} \boldsymbol{F}_k^T$ の由来
ランダムベクトル $\boldsymbol{z}$ が共分散 $\boldsymbol{P}$ を持つとき、線形変換 $\boldsymbol{F} \boldsymbol{z}$ の共分散は $\mathbb{E}[(\boldsymbol{F}\boldsymbol{z})(\boldsymbol{F}\boldsymbol{z})^T] = \boldsymbol{F} \mathbb{E}[\boldsymbol{z}\boldsymbol{z}^T] \boldsymbol{F}^T = \boldsymbol{F} \boldsymbol{P} \boldsymbol{F}^T$ となる。$\boldsymbol{w}_k$ は $\boldsymbol{x}_{k-1}$ と独立なので、和の共分散は両者の和になる。
更新ステップの詳細
更新ステップでは、観測値を使って予測を補正する。
残差（イノベーション）
\begin{equation} \boldsymbol{e}_k = \boldsymbol{y}_k - \boldsymbol{H}_k \hat{\boldsymbol{x}}_{k|k-1} \end{equation}実際の観測値と予測された観測値の差。この差を使って状態推定を補正する。
残差共分散
\begin{equation} \boldsymbol{S}_k = \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T + \boldsymbol{R}_k \end{equation}残差の分散共分散行列。予測誤差と観測ノイズの両方を含む。
Kalmanゲイン
\begin{equation} \boldsymbol{K}_k = \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T \boldsymbol{S}_k^{-1} \end{equation}予測と観測のどちらをどれだけ信頼するかを決める重み。
- 観測ノイズ $\boldsymbol{R}_k$ が大きい → $\boldsymbol{K}_k$ が小さくなり、予測を重視
- 予測誤差 $\boldsymbol{P}_{k|k-1}$ が大きい → $\boldsymbol{K}_k$ が大きくなり、観測を重視
状態更新
\begin{equation} \hat{\boldsymbol{x}}_{k|k} = \hat{\boldsymbol{x}}_{k|k-1} + \boldsymbol{K}_k \boldsymbol{e}_k \end{equation}予測値に残差をKalmanゲインで重み付けして加える。
共分散更新
\begin{equation} \boldsymbol{P}_{k|k} = (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k) \boldsymbol{P}_{k|k-1} \end{equation}観測値を取り入れることで、推定の不確かさが減少する。
Joseph 形式 (数値安定版)
数値的安定性を高めるため、以下の等価な形式もよく使われる：
\begin{equation} \boldsymbol{P}_{k|k} = (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k) \boldsymbol{P}_{k|k-1} (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k)^T + \boldsymbol{K}_k \boldsymbol{R}_k \boldsymbol{K}_k^T \end{equation}標準形 $(\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k) \boldsymbol{P}_{k|k-1}$ と等価であることは、最適な $\boldsymbol{K}_k$ を代入すれば示せる (「導出」ページ参照)。標準形は行列積の結果が数値誤差で対称性を失いやすいのに対し、Joseph 形は $A\boldsymbol{P}A^T + B\boldsymbol{R}B^T$ という二次形式の和で書かれるため、機械精度の範囲で常に対称かつ半正定値性が保たれる。任意の $\boldsymbol{K}_k$ (最適でなくても) に対して正しい共分散を与える点も実装上の利点である。
初期化について
Kalmanフィルタを開始するには、初期状態推定値 $\hat{\boldsymbol{x}}_{0|-1}$ と初期誤差共分散行列 $\boldsymbol{P}_{0|-1}$ を設定する必要がある。
初期状態の設定
- 事前情報がある場合：その情報に基づいて設定
- 事前情報がない場合：ゼロベクトルや最初の観測値に基づく推定値を使用
初期共分散の設定
- 不確かさが大きい場合：大きな値（例：$\boldsymbol{P}_0 = 1000 \boldsymbol{I}$）
- 初期状態に確信がある場合：小さな値
初期化の影響
初期値の設定が悪くても、十分なデータがあればKalmanフィルタは正しい推定に収束する。 ただし、初期共分散を過小評価すると収束が遅くなる可能性がある。
よくある質問（FAQ）
- カルマンフィルタの2つの計算ステップとは何ですか？
- 予測ステップと更新ステップである。予測ステップでは状態遷移モデルで次の状態と誤差共分散を予測する。更新ステップでは実際の観測値からカルマンゲインを計算し、状態と共分散を補正する。この2ステップを繰り返すことで最適推定を実現する。
- カルマンゲインとは何ですか？
- カルマンゲイン $K$ は予測誤差共分散 $P$ と観測雑音共分散 $R$ の比で決まる重みである。$K = PH^T(HPH^T + R)^{-1}$ で計算される。予測の不確かさ（$P$ 大）が大きいほど観測値を重視し、観測雑音（$R$ 大）が大きいほど予測値を重視する。
- Joseph 形式の共分散更新とは何ですか？
- Joseph 形式は $(I - KH)P(I - KH)^T + KRK^T$ という二次形式で共分散を更新する方法である。標準形 $(I-KH)P$ と等価だが、行列積の丸め誤差による対称性・半正定値性の崩れを防ぐため、数値安定性が高い実装に推奨される。