概要
カルマンフィルタ（Kalman Filter）は、センサの測定値のような誤差を含む観測値に対して、システムの数学的モデルによる予測値との重みづけを行うことで真の状態を推定する手法である[1]。事後推定誤差共分散行列のトレースを最小化するような重みづけの係数（カルマンゲイン）を決定する。
システムモデル
システムの連続時間状態空間モデル（状態方程式+出力方程式）は一般的に次のように定義される。
\[\begin{eqnarray}
\dot{\boldsymbol{x}}(t) & = & \mathbf{A}\boldsymbol{x}(t)+\mathbf{B}\boldsymbol{u}(t) \\
\boldsymbol{y}(t) & = & \mathbf{C}\boldsymbol{x}(t)
\end{eqnarray}\]
ただし、\(\boldsymbol{x}\)は状態、\(\boldsymbol{u}\)は入力、\(\boldsymbol{y}\)は出力、\(\mathbf{A}\)は状態行列、\(\mathbf{B}\)は入力行列、\(\mathbf{C}\)は出力行列である。
この連続時間状態空間モデルを離散化したとき、離散時間状態空間モデル（状態方程式+観測方程式）は次のように表されるとする。
\[\begin{eqnarray}
\boldsymbol{x}_{k} & = & \mathbf{F}_{k}\boldsymbol{x}_{k-1} + \mathbf{B}_{k}\boldsymbol{u}_{k} + \boldsymbol{w}_{k} \\
\boldsymbol{z}_{k} & = & \mathbf{H}_{k}\boldsymbol{x}_{k} + \boldsymbol{v}_{k}
\end{eqnarray}\]
ただし、\(\boldsymbol{z}_{k}\)は観測、\(\mathbf{F}_{k}\)は状態遷移行列、\(\mathbf{H}_{k}\)は観測行列、\(\boldsymbol{w}_{k} \sim \mathcal{N}(\mathbf{0},\mathbf{Q}_{k})\)と\(\boldsymbol{v}_{k} \sim \mathcal{N}(\mathbf{0},\mathbf{R}_{k})\)はノイズである。
補足：\(\mathcal{N}(\boldsymbol{\mu},\mathbf{\Sigma})\)は平均\(\boldsymbol{\mu}\)、共分散行列\(\mathbf{\Sigma}\)の多変量正規分布に従うノイズ
フィルタ動作
カルマンフィルタは、予測と更新の2つのステップから成る。予測ステップでは、離散化された状態方程式を用いてある過去の状態から現在の状態を推定する。予測ステップで推定された状態の値は事前（priori）推定値と呼ばれる。更新ステップでは事前推定値と観測値の差分である観測残差の重みを適切に設定することで事後（posteriori）推定値を決定する。この事後推定値がカルマンフィルタの結果になる。
予測
時刻\(k\)における状態の真値を\(\boldsymbol{x}_{k}\)、時刻\(m\)における時刻\(n\)の状態推定値を\(\boldsymbol{x}_{m|n}\)、時刻\(m\)における時刻\(n\)の推定値の誤差共分散行列を\(\mathbf{P}_{m|n}\)とする。
事前状態推定値
\[\hat{\boldsymbol{x}}_{k|k-1} = \mathbf{F}_{k}\hat{\boldsymbol{x}}_{k-1|k-1} + \mathbf{B}_{k}\boldsymbol{u}_{k}\]
事前推定誤差共分散行列
\[\mathbf{P}_{k|k-1} = \mathbf{F}_{k}\mathbf{P}_{k-1|k-1}\mathbf{F}^{\mathrm{T}}_{k} + \mathbf{Q}_{k}\]
更新
事後状態推定値
\[\hat{\boldsymbol{x}}_{k|k} = (\mathbf{I} – \mathbf{K}_k \mathbf{H}_k) \hat{\boldsymbol{x}}_{k|k-1} + \mathbf{K}_k \boldsymbol{z}_k\]
事後推定誤差共分散行列
\[\mathbf{P}_{k|k} = (\mathbf{I} – \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1}\]
カルマンゲイン
\[\mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^{\mathrm{T}} \mathbf{S}_k^{-1}\]
観測残差
\[\boldsymbol{e}_k = \boldsymbol{z}_k – \mathbf{H}_k \hat{\boldsymbol{x}}_{k|k-1}\]
観測残差共分散行列
\[\mathbf{S}_k = \mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^{\mathrm{T}} + \mathbf{R}_k\]
導出
事前推定誤差共分散行列
\[
\begin{eqnarray}
\mathbf{P}_{k|k-1} & = & \mathrm{cov}(\boldsymbol{x}_{k}-\hat{\boldsymbol{x}}_{k|k-1}) \\
& = & \mathrm{cov}((\mathbf{F}_{k}\boldsymbol{x}_{k-1} + \mathbf{B}_{k}\boldsymbol{u}_{k} + \boldsymbol{w}_{k})-(\mathbf{F}_{k}\hat{\boldsymbol{x}}_{k-1|k-1} + \mathbf{B}_{k}\boldsymbol{u}_{k})) \\
& = & \mathrm{cov}(\mathbf{F}_{k}(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1|k-1}) + \boldsymbol{w}_{k}) \\
& = & \mathrm{E}[(\mathbf{F}_{k}(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1|k-1}) + \boldsymbol{w}_{k})(\mathbf{F}_{k}(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1|k-1}) + \boldsymbol{w}_{k})^{\mathrm{T}}] \\
& = & \mathrm{E}[\mathbf{F}_{k}(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1|k-1})(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1|k-1})^{\mathrm{T}}\mathbf{F}_{k}^{\mathrm{T}} + \boldsymbol{w}_{k}\boldsymbol{w}_{k}^{\mathrm{T}}] \\
& = & \mathbf{F}_{k}\mathrm{E}[(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1|k-1})(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1|k-1})^{\mathrm{T}}]\mathbf{F}_{k}^{\mathrm{T}} + \mathrm{E}[\boldsymbol{w}_{k}\boldsymbol{w}_{k}^{\mathrm{T}}] \\
& = & \mathbf{F}_{k}\mathrm{cov}(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1|k-1})\mathbf{F}_{k}^{\mathrm{T}} + \mathrm{cov}(\boldsymbol{w}_{k}) \\
& = & \mathbf{F}_{k}\mathbf{P}_{k-1|k-1}\mathbf{F}_{k}^{\mathrm{T}} + \mathbf{Q}_{k}
\end{eqnarray}
\]
ただし、\(\mathbf{P}_{k-1|k-1} = \mathrm{cov}(\boldsymbol{x}_{k-1} – \hat{\boldsymbol{x}}_{k-1|k-1})\)、\(\mathrm{cov}(\boldsymbol{w}_k) = \mathbf{Q}_k\)であり、事前状態推定値の誤差とノイズは独立であるからその期待値が
\[\mathrm{E}[(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1|k-1})\boldsymbol{w}_{k}^{\mathrm{T}}] = \mathrm{E}[(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1|k-1})^{\mathrm{T}}\boldsymbol{w}_{k}] = 0\]
となることを利用している。
補足：\(\mathrm{cov}(\boldsymbol{x}) = \mathrm{E}[\boldsymbol{x}\boldsymbol{x}^{\mathrm{T}}]\)（共分散行列と期待値の関係）
事後状態推定値
\[
\begin{eqnarray}
\hat{\boldsymbol{x}}_{k|k} & = & \hat{\boldsymbol{x}}_{k|k-1} + \mathbf{K}_k \boldsymbol{e}_k \\
& = & \hat{\boldsymbol{x}}_{k|k-1} + \mathbf{K}_k (\boldsymbol{z}_k – \mathbf{H}_k \hat{\boldsymbol{x}}_{k|k-1}) \\
& = & (\mathbf{I} – \mathbf{K}_k \mathbf{H}_k) \hat{\boldsymbol{x}}_{k|k-1} + \mathbf{K}_k \boldsymbol{z}_k
\end{eqnarray}
\]
事後推定誤差共分散行列
\[\begin{eqnarray} \mathbf{P}_{k|k} & = & \mathrm{cov}(\boldsymbol{x}_{k} – \hat{\boldsymbol{x}}_{k|k}) \\
& = & \mathrm{cov}(\boldsymbol{x}_{k} – (\hat{\boldsymbol{x}}_{k|k-1} + \mathbf{K}_{k}(\boldsymbol{z}_{k}- \mathbf{H}_{k}\hat{\boldsymbol{x}}_{k|k-1}))) \\
& = & \mathrm{cov}((\mathbf{I} – \mathbf{K}_{k}\mathbf{H}_{k})(\boldsymbol{x}_{k} – \hat{\boldsymbol{x}}_{k|k-1}) – \mathbf{K}_{k}\boldsymbol{v}_{k}) \\
& = & \mathrm{E}[((\mathbf{I} – \mathbf{K}_{k}\mathbf{H}_{k})(\boldsymbol{x}_{k} – \hat{\boldsymbol{x}}_{k|k-1}) – \mathbf{K}_{k}\boldsymbol{v}_{k})((\mathbf{I} – \mathbf{K}_{k}\mathbf{H}_{k})(\boldsymbol{x}_{k} – \hat{\boldsymbol{x}}_{k|k-1}) – \mathbf{K}_{k}\boldsymbol{v}_{k})^{\mathrm{T}}] \\
& = & \mathrm{E}[(\mathbf{I} – \mathbf{K}_{k}\mathbf{H}_{k})(\boldsymbol{x}_{k} – \hat{\boldsymbol{x}}_{k|k-1})(\boldsymbol{x}_{k} – \hat{\boldsymbol{x}}_{k|k-1})^{\mathrm{T}}(\mathbf{I} – \mathbf{K}_{k}\mathbf{H}_{k})^{\mathrm{T}} + \mathbf{K}_{k}\boldsymbol{v}_{k}\boldsymbol{v}_{k}^{\mathrm{T}}\mathbf{K}_{k}^{\mathrm{T}}] \\
& = & (\mathbf{I} – \mathbf{K}_{k}\mathbf{H}_{k})\mathrm{E}[(\boldsymbol{x}_{k} – \hat{\boldsymbol{x}}_{k|k-1})(\boldsymbol{x}_{k} – \hat{\boldsymbol{x}}_{k|k-1})^{\mathrm{T}}](\mathbf{I} – \mathbf{K}_{k}\mathbf{H}_{k})^{\mathrm{T}} + \mathbf{K}_{k}\mathrm{E}[\boldsymbol{v}_{k}\boldsymbol{v}_{k}^{\mathrm{T}}]\mathbf{K}_{k}^{\mathrm{T}} \\
& = & (\mathbf{I} – \mathbf{K}_{k}\mathbf{H}_{k})\mathrm{cov}(\boldsymbol{x}_{k} – \hat{\boldsymbol{x}}_{k|k-1})(\mathbf{I} – \mathbf{K}_{k}\mathbf{H}_{k})^{\mathrm{T}} + \mathbf{K}_{k}\mathrm{cov}(\boldsymbol{v}_{k})\mathbf{K}_{k}^{\mathrm{T}} \\
& = & (\mathbf{I} – \mathbf{K}_{k}\mathbf{H}_{k})\mathbf{P}_{k|k-1}(\mathbf{I} – \mathbf{K}_{k}\mathbf{H}_{k})^{\mathrm{T}} + \mathbf{K}_{k}\mathbf{R}_{k}\mathbf{K}_{k}^{\mathrm{T}} \end{eqnarray}\]
ただし、\(\mathbf{P}_{k|k-1} = \mathrm{cov}(\boldsymbol{x}_{k} – \hat{\boldsymbol{x}}_{k|k-1})\)、\(\mathrm{cov}(\boldsymbol{v}_k) = \mathbf{R}_k\)であり、事前状態推定値の誤差とノイズは独立であるからその期待値が
\[\mathrm{E}[(\boldsymbol{x}_{k}-\hat{\boldsymbol{x}}_{k|k})\boldsymbol{v}_{k}^{\mathrm{T}}] = \mathrm{E}[(\boldsymbol{x}_{k}-\hat{\boldsymbol{x}}_{k|k})^{\mathrm{T}}\boldsymbol{v}_{k}]=0\]
となることを利用している。
補足：\(\mathrm{cov}(\boldsymbol{x}) = \mathrm{E}[\boldsymbol{x}\boldsymbol{x}^{\mathrm{T}}]\)（共分散行列と期待値の関係）
\[
\begin{eqnarray}
\mathbf{P}_{k|k} & = & (\mathbf{I} – \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1} (\mathbf{I} – \mathbf{K}_k \mathbf{H}_k)^{\mathrm{T}} + \mathbf{K}_k \mathbf{R}_k \mathbf{K}_k^T \\
& = & \mathbf{P}_{k|k-1} – \mathbf{K}_k \mathbf{H}_k \mathbf{P}_{k|k-1} – \mathbf{P}_{k|k-1} \mathbf{H}_k^{\mathrm{T}} \mathbf{K}_k^{\mathrm{T}} + \mathbf{K}_k (\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T + \mathbf{R}_k) \mathbf{K}_k^{\mathrm{T}}
\end{eqnarray}
\]
最後の項について、観測残差共分散行列とカルマンゲインの式を用いれば、
\[
\begin{eqnarray}
\mathbf{K}_k (\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^{\mathrm{T}} + \mathbf{R}_k) \mathbf{K}_k^{\mathrm{T}} & = & \mathbf{K}_k \mathbf{S}_k \mathbf{K}_k^{\mathrm{T}} \\
& = & \mathbf{P}_{k|k-1}\mathbf{H}^{\mathrm{T}}\mathbf{S}_{k}^{-1} \mathbf{S}_k \mathbf{K}_k^{\mathrm{T}} \\
& = & \mathbf{P}_{k|k-1}\mathbf{H}^{\mathrm{T}}\mathbf{K}_k^{\mathrm{T}}
\end{eqnarray}
\]
となる。したがって、
\[\mathbf{P}_{k|k} = (\mathbf{I} – \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1}\]
カルマンゲイン
事後推定誤差共分散行列のトレースは、以下のように表される。
\[
\begin{eqnarray}
\mathrm{tr}(\mathbf{P}_{k|k}) & = & \mathrm{tr}((\mathbf{I}-\mathbf{K}_k\mathbf{H}_k)\mathbf{P}_{k|k-1}(\mathbf{I}-\mathbf{K}_k\mathbf{H}_k)^{\mathrm{T}}) + \mathrm{tr}(\mathbf{K}_k \mathbf{R}_k \mathbf{K}_k^{\mathrm{T}}) \\
& = & \mathrm{tr}(\mathbf{P}_{k|k-1}-\mathbf{K}_k \mathbf{H}_k \mathbf{P}_{k|k-1}-\mathbf{P}_{k|k-1} \mathbf{H}_k^{\mathrm{T}} \mathbf{K}_k^{\mathrm{T}} + \mathbf{K}_k \mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^{\mathrm{T}} \mathbf{K}_k^{\mathrm{T}} + \mathbf{K}_k \mathbf{R}_k \mathbf{K}_k^{\mathrm{T}}) \\
& = & \mathrm{tr}(\mathbf{P}_{k|k-1})-2 \mathrm{tr}(\mathbf{K}_k \mathbf{H}_k \mathbf{P}_{k|k-1}) + \mathrm{tr}(\mathbf{K}_k (\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^{\mathrm{T}} + \mathbf{R}_k) \mathbf{K}_k^{\mathrm{T}})
\end{eqnarray}
\]
ただし、\(\mathrm{tr}(\mathbf{A}^{\mathrm{T}})=\mathrm{tr}(\mathbf{A})\)であること、また\(\mathrm{tr}(\mathbf{P}_{k|k})\)が対称行列であることを用いている。
\(\mathbf{K}_{k}\)に対して2次形式となる項の係数が正であることから、\(\mathrm{tr}(\mathbf{P}_{k|k})\)はその微分値が0となる点で最小値をとる。したがって、
\[\frac{\partial \, \mathrm{tr}(\mathbf{P}_{k|k})}{\partial \mathbf{K}_k} = -2 \mathbf{P}_{k|k-1} \mathbf{H}_k^{\mathrm{T}} + 2 \mathbf{K}_k (\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^{\mathrm{T}} + \mathbf{R}_k) = 0\]
\[\Rightarrow \mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^{\mathrm{T}} (\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^{\mathrm{T}} + \mathbf{R}_k)^{-1}\]
観測残差共分散行列を用いて表せば、カルマンゲインは
\[\mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^{\mathrm{T}} \mathbf{S}_k^{-1}\]
となる。
ただし、トレースに関して以下の性質を利用している。
\[\frac{\partial \, \mathrm{tr}(\mathbf{A} \mathbf{X})}{\partial \mathbf{X}} = \mathbf{A}^{\mathrm{T}}\]
\[\frac{\partial \, \mathrm{tr}(\mathbf{X} \mathbf{A} \mathbf{X}^{\mathrm{T}})}{\partial \mathbf{X}} = (\mathbf{A}+\mathbf{A}^{\mathrm{T}}) \mathbf{X}\]
\(\mathbf{A}\)が対称行列ならば、\(\mathbf{A} = \mathbf{A}^{\mathrm{T}}\)より、
\[\frac{\partial \, \mathrm{tr}(\mathbf{X} \mathbf{A} \mathbf{X}^{\mathrm{T}})}{\partial \mathbf{X}} = 2\mathbf{A}\mathbf{X}\]
観測残差共分散行列
\[
\begin{eqnarray}
\mathbf{S}_k & = & \mathrm{cov}(\boldsymbol{e}_{k}) \\
& = & \mathrm{cov}(\boldsymbol{z}_k-\mathbf{H}_k \hat{\boldsymbol{x}}_{k|k-1}) \\
& = & \mathrm{cov}(\mathbf{H}_{k}(\boldsymbol{x}_{k}-\hat{\boldsymbol{x}}_{k|k-1}) + \boldsymbol{v}_k) \\
& = & \mathrm{E}[(\mathbf{H}_{k}(\boldsymbol{x}_{k}-\hat{\boldsymbol{x}}_{k|k-1}) + \boldsymbol{v}_k)(\mathbf{H}_{k} (\boldsymbol{x}_{k}-\hat{\boldsymbol{x}}_{k|k-1}) + \boldsymbol{v}_k)^{\mathrm{T}}] \\
& = & \mathrm{E}[(\mathbf{H}_{k}(\boldsymbol{x}_{k}-\hat{\boldsymbol{x}}_{k|k-1})(\boldsymbol{x}_{k}-\hat{\boldsymbol{x}}_{k|k-1})^{\mathrm{T}}\mathbf{H}_{k}^{\mathrm{T}} + \boldsymbol{v}_k\boldsymbol{v}_k^{\mathrm{T}}] \\
& = & \mathbf{H}_{k}\mathrm{E}[(\boldsymbol{x}_{k}-\hat{\boldsymbol{x}}_{k|k-1})(\boldsymbol{x}_{k}-\hat{\boldsymbol{x}}_{k|k-1})^{\mathrm{T}}]\mathbf{H}_{k}^{\mathrm{T}} + \mathrm{E}[\boldsymbol{v}_k\boldsymbol{v}_k^{\mathrm{T}}] \\
& = & \mathbf{H}_{k}\mathrm{cov}(\boldsymbol{x}_{k}-\hat{\boldsymbol{x}}_{k|k-1})\mathbf{H}_{k}^{\mathrm{T}} + \mathrm{cov}(\boldsymbol{v}_k) \\
& = & \mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^{\mathrm{T}} + \mathbf{R}_k
\end{eqnarray}
\]
ただし、\(\mathbf{P}_{k|k-1} = \mathrm{cov}(\boldsymbol{x}_{k} – \hat{\boldsymbol{x}}_{k|k-1})\)、\(\mathrm{cov}(\boldsymbol{v}_k) = \mathbf{R}_k\)であり、事前状態推定値の誤差とノイズは独立であるからその期待値が
\[\mathrm{E}[(\boldsymbol{x}_{k}-\hat{\boldsymbol{x}}_{k|k})\boldsymbol{v}_{k}^{\mathrm{T}}] = \mathrm{E}[(\boldsymbol{x}_{k}-\hat{\boldsymbol{x}}_{k|k})^{\mathrm{T}}\boldsymbol{v}_{k}]=0\]
となることを利用している。
補足：\(\mathrm{cov}(\boldsymbol{x}) = \mathrm{E}[\boldsymbol{x}\boldsymbol{x}^{\mathrm{T}}]\)（共分散行列と期待値の関係）
応用
カルマンフィルタは、アポロ計画におけるロケットに使用された[2]。上記のカルマンフィルタは線形システムに対するものであるが、ロケットの力学モデルは非線形であるため、対象付近において線形近似を行う拡張カルマンフィルタ（Extended Kalman Filter, EKF）を採用していた。
参考文献
[1] R. E. Kalman (1960) “A New Approach to Linear Filtering and Prediction Problems” Journal of Basic Engineering (ASME), 82(Series D), pp. 35–45
[2] L. A. McGee and S. F. Schmidt (1985) “Discovery of the Kalman Filter as a Practical Tool for Aerospace and Industry” NASA Technical Memorandum NASA-TM-86847, National Aeronautics and Space Administration.