Kalmanフィルタ
Kalmanフィルタとは
Kalmanフィルタ（カルマンフィルタ）は、ノイズを含む観測データから、 システムの内部状態を最適に推定するアルゴリズムである。 1960年にRudolf E. Kalmanが発表した論文に基づく。
【連続時間への拡張】R. E. Kalman, R. S. Bucy, "New Results in Linear Filtering and Prediction Theory", ASME Journal of Basic Engineering, 83(1): 95–108 (1961).
Kalmanフィルタは以下の特徴を持つ：
- 逐次処理：データが到着するたびにリアルタイムで推定を更新
- 最適性：線形ガウスシステムにおいて、最小分散推定を与える
- 計算効率：過去のデータをすべて保持する必要がなく、メモリ効率が良い
- 予測能力：現在の推定だけでなく、将来の状態も予測可能
歴史的背景
Kalmanフィルタは1960年にRudolf E. Kálmánが発表し、1960年代のアポロ計画で軌道推定に採用されたことで広く知られるようになった。 以来、GPS、自動運転、ロボット工学など、現代のほぼすべての制御・推定システムで使われている。
状態空間モデル
Kalmanフィルタは以下の状態空間モデルを仮定する。
\begin{align} \text{状態方程式} : \quad \boldsymbol{x}_k &= \boldsymbol{F}_k \boldsymbol{x}_{k-1} + \boldsymbol{w}_k \\ \text{観測方程式} : \quad \boldsymbol{y}_k &= \boldsymbol{H}_k \boldsymbol{x}_k + \boldsymbol{v}_k \end{align}ここで：
- $\boldsymbol{x}_k$：時刻 $k$ の状態ベクトル（推定したい未知量）
- $\boldsymbol{y}_k$：時刻 $k$ の観測ベクトル（センサで得られるデータ）
- $\boldsymbol{F}_k$：状態遷移行列（システムのダイナミクス）
- $\boldsymbol{H}_k$：観測行列（状態から観測への変換）
- $\boldsymbol{w}_k$：プロセスノイズ（モデル化できない外乱、$\boldsymbol{Q}_k = E[\boldsymbol{w}_k\boldsymbol{w}_k^T]$）
- $\boldsymbol{v}_k$：観測ノイズ（センサの測定誤差、$\boldsymbol{R}_k = E[\boldsymbol{v}_k\boldsymbol{v}_k^T]$）
Kalmanフィルタの構成
Kalmanフィルタは予測と更新の2ステップを交互に実行する：
- 予測（Predict）：前回の推定値から現在の状態を予測
- 更新（Update）：観測値を使って予測を補正
Kalmanゲイン $\boldsymbol{K}_k$ の役割
図2の減算器が出力する $\boldsymbol{e}_k = \boldsymbol{y}_k - \hat{\boldsymbol{y}}_k$ はイノベーション（innovation）と呼ばれ、 「予測がどれだけ外れたか」を表す。Kalmanゲイン $\boldsymbol{K}_k$ は、このイノベーションをどれだけ信頼して推定値を修正するかを決める重みである。
最小化対象はイノベーション $\boldsymbol{e}_k$ そのものではなく、 推定誤差共分散のトレース（各状態変数の推定誤差分散の合計）である：
$$\text{tr}(\boldsymbol{P}_{k|k}) \to \min, \quad \boldsymbol{P}_{k|k} = E\!\left[(\boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k})(\boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k})^T\right]$$この最小化から導かれる最適ゲインは次のようになる（導出の詳細は導出を参照）：
$$\boldsymbol{K}_k = \boldsymbol{P}_{k|k-1}\boldsymbol{H}_k^T(\boldsymbol{H}_k\boldsymbol{P}_{k|k-1}\boldsymbol{H}_k^T + \boldsymbol{R}_k)^{-1}$$直感的には：
- 観測ノイズが大きい（$\boldsymbol{R}_k$ が大きい）→ $\boldsymbol{K}_k$ が小さくなる → 観測をあまり信頼せず、予測を重視
- 予測の不確かさが大きい（$\boldsymbol{P}_{k|k-1}$ が大きい）→ $\boldsymbol{K}_k$ が大きくなる → 観測を重視して大きく修正
Wienerフィルタとの対比
Wienerフィルタの最適解 $G(\omega) = H^*P_S(|H|^2P_S + P_N)^{-1}$ と比較すると、 Kalmanゲインも「信号の不確かさ」対「ノイズの不確かさ」の比で重みを決めるという同じ原理に基づいている。 Wienerフィルタが周波数領域で信号対ノイズ比を使うのに対し、Kalmanフィルタは時間領域で共分散行列の比を使う。
Kalmanフィルタの前提条件と拡張
既知でなければならないもの
基本Kalmanフィルタを適用するには、以下のパラメータが既知（または事前に設定）である必要がある：
| パラメータ | 意味 | 実務上の扱い |
|---|---|---|
| $\boldsymbol{F}_k$ | 状態遷移行列 | 物理法則から導出（例：運動方程式） |
| $\boldsymbol{H}_k$ | 観測行列 | センサの特性から決定 |
| $\boldsymbol{Q}_k$ | プロセスノイズ共分散 | モデル誤差の大きさ（チューニングが必要） |
| $\boldsymbol{R}_k$ | 観測ノイズ共分散 | センサ仕様または実測から決定 |
| $\hat{\boldsymbol{x}}_0,\, \boldsymbol{P}_0$ | 初期推定値と共分散 | 事前知識または大きな $\boldsymbol{P}_0$ で初期化 |
添字 $k$ について
表中の添字 $k$ は「時変システムにも対応できる」という一般性のために付いている。 実用上の多くのシステムは時不変（LTI）であり、$\boldsymbol{F}, \boldsymbol{H}, \boldsymbol{Q}, \boldsymbol{R}$ は定数行列として一度決めれば全時刻で同じである（GPS、慣性航法、多くのセンサフュージョンなど）。 一方、ロケットの飛行中（燃料消費で質量が変化）や航空機の旋回中（運動方程式が変化）のように、システムのダイナミクスが時々刻々変わる場合は、各時刻 $k$ ごとに $\boldsymbol{F}_k$ を物理モデルから計算して与える必要がある。
実用上、これらすべてが正確にわかっていることは稀である。 特に $\boldsymbol{Q}_k$ と $\boldsymbol{R}_k$ は理論的に導出しにくく、試行錯誤的なチューニングが必要になることが多い。
パラメータが不正確だとどうなるか
- $\boldsymbol{Q}_k$ が過小 → フィルタがモデルを過信し、観測を無視する（発散のリスク）
- $\boldsymbol{Q}_k$ が過大 → 観測に追従しすぎてノイズを除去できない
- $\boldsymbol{R}_k$ が過小 → ノイズの多い観測を信頼しすぎる
- $\boldsymbol{R}_k$ が過大 → 観測を無視して予測に頼りすぎる
- $\boldsymbol{F}_k$ が不正確 → 予測自体が的外れになる
ノイズが非ガウスだとどうなるか
Kalmanフィルタはノイズがガウスでなくても動作はする。 ただし最適性の保証が変わる：
- ガウスノイズ：Kalmanフィルタは全推定器の中で最適（最小分散推定）
- 非ガウスノイズ：Kalmanフィルタは線形推定器の中では最適（BLUE: Best Linear Unbiased Estimator）だが、非線形推定器にはかなわない場合がある
| ノイズの種類 | Kalmanフィルタの性能 | より適した手法 |
|---|---|---|
| ガウス | 最適（これ以上は不可能） | なし |
| 裾の重い分布（外れ値が多い） | 外れ値に引きずられる | パーティクルフィルタ、ロバストKF |
| 多峰性（複数の仮説） | 単一ガウスで近似するため破綻 | パーティクルフィルタ、Mixture KF |
| 一様分布 | 動くが非効率 | パーティクルフィルタ |
つまり「使えなくなる」のではなく「最適ではなくなる」。 実用上、ノイズが概ねガウスに近ければ十分に機能する。 非ガウス性が顕著な場合はパーティクルフィルタなどの非線形手法が有効である。
Wienerフィルタからの飛躍
以上の前提条件や限界を踏まえた上で、Kalmanフィルタの歴史的な意義を振り返る。 Kalmanフィルタ以前の最適フィルタリングは、1940年代にNorbert Wienerが開発したWienerフィルタが主流であった。 Wienerフィルタは周波数領域で定式化され、定常過程に対して最小平均二乗誤差（MMSE）の意味で最適な線形フィルタを与える。 しかし実用上、いくつかの本質的な制約があった。
| 特性 | Wienerフィルタ | Kalmanフィルタ |
|---|---|---|
| 処理方式 | 標準的にはバッチ処理（全データが必要） | 逐次処理（1観測ずつリアルタイム更新） |
| 対応するシステム | 定常過程のみ | 非定常・時変システムにも対応 |
| 定式化 | 周波数領域（パワースペクトル） | 状態空間（時間領域、多変数に自然に拡張） |
| メモリ | 全観測履歴（または自己相関関数） | 現在の推定値と共分散のみ |
| 多変数 | 拡張が困難（MIMO Wiener-Hopf方程式） | 行列演算で自然に多変数対応 |
| 不確かさの定量化 | 残留誤差パワーのみ | 共分散行列で推定の信頼度を時々刻々追跡 |
つまりKalmanフィルタの本質的な革新は、最適フィルタリング問題を「スペクトル分解」から「状態推定の逐次更新」へと再定式化したことにある。 Wienerフィルタが「全データを見てから最適な周波数応答を設計する」のに対し、 Kalmanフィルタは「新しい観測が来るたびに推定を最適に修正する」。 この発想の転換により、リアルタイム制御・非定常システム・多変数問題という現実世界の要求に応えられるようになった。
両者の関係
定常過程で時刻 $k \to \infty$ の定常状態では、Kalmanゲイン $\boldsymbol{K}_k$ は一定値に収束し、 その定常KalmanフィルタはWienerフィルタと等価になる。 すなわちWienerフィルタはKalmanフィルタの定常極限として位置づけられる。
なぜ一人の研究者がこれほど多くの利点を同時に実現できたのか
Kalmanが1960年の論文 "A New Approach to Linear Filtering and Prediction Problems" で行ったのは、 個々の利点を1つずつ設計したのではなく、問題の定式化を根本的に変えたことである。 具体的には、Wienerの問題設定（周波数領域・定常・無限時間）を、 制御理論の状態空間表現（時間領域・有限次元・逐次更新）で再定式化した。
すると、先の表に挙げた利点はすべて自動的に導かれる：
| 利点 | なぜ状態空間表現から自動的に得られるか |
|---|---|
| 逐次処理 | 状態方程式 $\boldsymbol{x}_k = \boldsymbol{F}_k\boldsymbol{x}_{k-1} + \boldsymbol{w}_k$ が1ステップずつの更新構造を持つ |
| 非定常対応 | $\boldsymbol{F}_k, \boldsymbol{H}_k$ に時刻添字 $k$ が付いており、時変は一般化ではなく最初から含まれている |
| 時間領域定式化 | 状態空間表現そのものが時間領域の枠組みである |
| メモリ効率 | 状態ベクトル $\boldsymbol{x}_k$ が過去の全観測の情報を圧縮する十分統計量になっている |
| 多変数対応 | 状態ベクトルが多次元であることは最初から想定されている |
| 共分散追跡 | 誤差共分散 $\boldsymbol{P}_k$ の更新式が状態空間の枠組みで自然に導出される |
つまりKalmanの本質的な貢献は、多数の利点を個別に考案したことではなく、 「見方を変えたら全部ついてきた」という点にある。 適切な抽象化が一挙に問題を解決した、数学における定式化の力を示す好例である。
背景にあった知的土壌
- 1950年代の制御理論の発展、特にR.E. Bellmanの動的計画法（最適制御の逐次的な定式化）
- Wiener-Hopf理論の限界（非定常・多変数への拡張の困難さ）が広く認識されていた
- Kalman自身が制御理論と確率論・最適化の両方に精通していた
モデルが不完全な場合の拡張手法
基本Kalmanフィルタの制約（線形性、ガウス性、モデルパラメータの既知性）を緩和するために、多くの拡張手法が開発されている：
| 手法 | 何を推定・対処するか | 考え方 |
|---|---|---|
| 適応Kalmanフィルタ | $\boldsymbol{Q}_k$, $\boldsymbol{R}_k$ | イノベーション $\boldsymbol{e}_k$ の統計量を監視して共分散をオンラインで調整 |
| 拡張Kalmanフィルタ（EKF） | 非線形な状態遷移・観測 | 各時刻でヤコビアン（偏微分行列）により線形化 |
| 無香料Kalmanフィルタ（UKF） | 同上 | 線形化せず、シグマ点で非線形関数を近似 |
| パラメータ拡大法 | $\boldsymbol{F}$, $\boldsymbol{H}$ 内の未知パラメータ | 未知パラメータを状態ベクトルに追加して同時推定 |
| EMアルゴリズム | $\boldsymbol{F}$, $\boldsymbol{H}$, $\boldsymbol{Q}$, $\boldsymbol{R}$ | バッチ的にモデルパラメータを最尤推定 |
| パーティクルフィルタ | 任意の非線形・非ガウス | モンテカルロ法でガウス仮定を撤廃 |
実務上の指針
- $\boldsymbol{F}_k$, $\boldsymbol{H}_k$ が線形で既知 → 基本Kalmanフィルタ
- $\boldsymbol{Q}_k$, $\boldsymbol{R}_k$ が不確か → 適応Kalmanフィルタで動的調整
- システムが非線形 → EKF（弱い非線形）または UKF（強い非線形）
- モデル構造自体が未知 → パラメータ拡大法またはEM
- 非ガウスノイズ → パーティクルフィルタ
記事一覧
応用分野
GPS/GNSS
衛星信号から位置・速度を高精度に推定
ロボット工学
SLAM（自己位置推定と地図作成の同時実行）
自動運転
センサフュージョンによる車両位置・姿勢の推定
航空宇宙
航空機・宇宙機の軌道推定・姿勢制御
金融
時系列データのトレンド推定・予測
画像処理
物体追跡・動画の手ブレ補正