カルマンフィルタをなるべく簡潔かつ直観的に導出する

時系列信号の推定・予測手法の基本中の基本であるカルマンフィルタを，直観的理解を見失わない範囲でなるべく簡潔に導出してみる試み．まあ実際に書き上げてみた結果「簡潔って何だっけ」という気持ちになってる．

直観的であることと簡潔であることはなかなか両立が難しい．実際，単に証明の字面を短くしたいならもっと別のアプローチ (例えば直交射影原理) の方がよいだろうし，今回のアプローチでもさらに冗長性を排除することはできる．しかし簡潔過ぎると初学者にはきつい (私にもきつい)．少々長くなるのは許容して，導出になっているのと同時に解説にもなるようなものを目指した．

一方で，長すぎるとそれはそれで心が折れるので，細かい話や念のための補足説明は，注釈としてグレーのボックスに分けて書くことにした．

この手の文章を書くときに，前提知識をどう設定するべきかはとても難しい．今回は，線形代数は完全に既習として，確率論についても，密度関数，同時確率，条件付き確率といった基本的なところは説明なしに使うことにした．共分散行列や多次元正規分布などの必要な知識については注釈にて証明なしに紹介している．

Too Long; Didn't Read

• 観測 $\v{y} = \m{H} \v{x} + \v{w}$ に基づく状態 $\v{x}$ の最適推定は $\hat{\v{x}} = (\m{H}\trans \m{R}\inv \m{H})\inv \m{H}\trans \m{R}\inv \v{y}$ で与えられる．ただし $\m{R}$ は $\v{w}$ の共分散
• $\v{y}$ が 2 つの独立な観測 $\v{y}_1 = \m{H}_1 \v{x} + \v{w}_1$ と $\v{y}_2 = \m{H}_2 \v{x} + \v{w}_2$ に分かれる場合，最適推定は $\hat{\v{x}} = (\m{H}_1\trans \m{R}_1\inv \m{H}_1 + \m{H}_2\trans \m{R}_2\inv \m{H}_2)\inv (\m{H}_1\trans \m{R}_1\inv \v{y}_1 + \m{H}_2\trans \m{R}_2\inv \v{y}_2)$ になる
• 前時刻からの予測によって見込まれる観測値を $\v{y}_1$，現時刻に得られた実際の観測値を $\v{y}_2$ とすれば，カルマンフィルタのできあがり

カルマンフィルタの問題設定

カルマンフィルタとは何なのか，どういう意味があるのか，どういうときに使うのかといった話は，ググればいっぱいで来るので省略．以下の問題を解くアルゴリズムをカルマンフィルタと呼ぶ．

推定したい状態を表す $\v{x}_k$ ($n$ 次元確率ベクトル) が時刻 $k = 0, 1, \ldots$ とともに以下のプロセスモデルに従って遷移するとする．ただし初期状態 $\v{x}_0$ は平均 $\v{\mu}_0$，共分散 $\m{P}_0$ の正規分布に従う． \begin{align} \v{x}_{k+1} &= \m{F}_k \v{x}_k + \v{v}_k \end{align} 各時刻 $k = 1, 2, \ldots$ において，観測値 $\v{y}_k$ ($m$ 次元確率ベクトル) が以下の観測モデルに従って得られるとする． \begin{align} \v{y}_k &= \m{H}_k \v{x}_k + \v{w}_k \end{align} ただしプロセス雑音 $\v{v}_k$ と観測雑音 $\v{w}_k$ は平均が $\v{0}$，共分散がそれぞれ $\m{Q}_k$，$\m{R}_k$ の正規分布に従う雑音である．プロセス雑音 $\v{v}_0, \v{v}_1, \ldots $，観測雑音 $\v{w}_1, \v{v}_2, \ldots $，および初期状態 $\v{x}_0$ は互いに独立とする．
行列 $\m{F}_k$, $\m{H}_k$, $\m{P}_k$, $\m{Q}_k$, $\m{R}_k$ とベクトル $\v{\mu}_0$ は既知の確定値とする．
時刻 $k$ までの観測値が得られたときの $\v{x}_k$ の最もよい推定値を得るためのアルゴリズムを導け．

• 推定対象である状態は，基本的には既知のモデル $\v{x}_{k+1} = \m{F}_k \v{x}_k$ に従って時間とともに変化する．ただし厳密にモデル通り動くとは限らない．モデルからのずれは加法的な正規雑音 $\v{v}_k$ として表現する．
• 観測値は，その時刻の状態から，基本的には既知のモデル $\v{y}_k = \m{H}_k \v{x}_k$ に従って得られる．ただし厳密にモデル通り得られるとは限らない．モデルからのずれは加法的な正規雑音 $\v{w}_k$ として表現する．
• 何が独立で何が独立でないのか混乱しそうになるが，図1 のように信号生成過程をブロック線図として描いてみるとわかりやすい．要するには外部から与えられるもの (青い文字のもの) はすべて互いに独立だと言っているだけである．

図1: カルマンフィルタが対象とする信号生成過程

期待値と共分散

• 確率ベクトル $\v{x}$ の期待値 (平均) は $\int \v{x} p(\v{x}) d\v{x}$ で与えられる．$\E[\v{x}]$ と書く．
  • 積分で定義されていることからわかるとおり線形である．例えば確定値の行列 $\m{A}$ とベクトル $\v{b}$ が与えられたとき $\m{A} \v{x} + \v{b}$ の $\v{x}$ に関する期待値は $\E[\m{A} \v{x} + \v{b}] = \m{A}\E[\v{x}] + \v{b}$，という具合に計算できる．この計算はよく使う．

• スカラ確率変数 $x$ の分散は $\E[(x - \bar{x})^2]$，ただし $\bar{x} = \E[x]$．分散は $x$ のばらつきの大きさの指標となる非負の数である．
• スカラ確率変数 $x$ と $y$ の共分散は $\E[(x - \bar{x})(y - \bar{y})]$ で定義される．$x$ と $y$ それぞれのばらつきの大きさを反映すると共に，両者の増えるか減るかの傾向の一致具合を反映している (一致するなら符号は正で，しないなら負)．
  • 相関係数のイメージがつかめている人は，その定義の分子に出てくるやつ (ばらつきの大きさで規格化する前のやつ) と理解してもよい．共分散がゼロということは相関がゼロであるということである．

• 確率ベクトル $\v{x}$ と $\v{y}$ に対して定義できるよう共分散を拡張すると，$\E[(\v{x} - \bar{\v{x}})(\v{y} - \bar{\v{y}})\trans]$ になる．
  • $(i, j)$-要素 が $x_i$ と $y_j$ の共分散であるような行列になっている．つまり要素間の共分散をすべて列挙したものになっている．これを共分散行列 (あるいは単に共分散) と呼ぶことがある．
• 共分散行列のうち特に $\v{x} = \v{y}$ の場合を考えると $\E[(\v{x} - \bar{\v{x}})(\v{x} - \bar{\v{x}})\trans]$ になる．これを $\v{x}$ の分散・共分散行列と呼ぶ (あるいはこれも共分散行列，さらに簡単に共分散とも呼ぶ) ．このエントリで単に「共分散」というときはこれを指している．
  • 対角要素には $\v{x}$ の要素それぞれの分散が，非対角要素には要素相互間の共分散が置かれる．対称行列になる．
  • $\v{x}$ と同次元の任意のベクトル $\v{m}$ について $\v{m}\trans \E[(\v{x} - \bar{\v{x}})(\v{x} - \bar{\v{x}})\trans] \v{m} = \E[\v{m}\trans (\v{x} - \bar{\v{x}})(\v{x} - \bar{\v{x}})\trans \v{m}] = \E[\|(\v{x} - \bar{\v{x}})\trans \v{m}\|^2] \geq 0$ なので，半正定行列である．
  • したがって適当な直交変換によって対角要素がすべて非負の対角行列にできる．言い換えると，任意の $\v{x}$ は，適当な直交変換によって要素間の相関がないベクトル $\v{x}'$ に表示し直すことができる．$\v{x}$ の共分散行列の固有値とは，無相関化された $\v{x}'$ の各要素の分散である．

独立性

• $\v{x}$ と $\v{y}$ が独立であるとは，$p(\v{x}, \v{y}) = p(\v{x}) p(\v{y})$ が成り立つことである．
  • $\v{y}$ が与えられたときの $\v{x}$ の条件付き確率密度 $p(\v{x} \given \v{y}) \defeq p(\v{x}, \v{y}) / p(\v{y})$ が $p(\v{x})$ に等しいことであると理解するとよい．
  • つまり $\v{y}$ を観測しても $\v{x}$ に関して何の情報も得られないことである．
• $\v{x}$ と $\v{y}$ が独立なら共分散 $\E[(\v{x} - \bar{\v{x}})(\v{y} - \bar{\v{y}})\trans]$ はゼロ (すなわち $\v{x}$ と $\v{y}$ は無相関) である．
  • $\iint (\v{x} - \bar{\v{x}})(\v{y} - \bar{\v{y}})\trans p(\v{x}, \v{y}) d\v{x} d\v{y} = \int (\v{x} - \bar{\v{x}}) p(\v{x}) d\v{x} \int (\v{y} - \bar{\v{y}})\trans p(\v{y}) d\v{y} = (\bar{\v{x}} - \bar{\v{x}})(\bar{\v{y}} - \bar{\v{y}})\trans = \m{O}$
  • その逆 (無相関 ⇒ 独立) は一般に成り立たない
• 独立なものにそれぞれどんなに演算を加えても独立のままである．
  • 何か適当な関数 $\v{f}(\cdot), \v{g}(\cdot)$ があったとして，$\v{y}$ を観測しても $\v{x}$ に関して何の情報も得られない状況で，$\v{f}(\v{y})$ を観測したところで $\v{g}(\v{x})$ についてはやっぱり何もわからない．
  • このことをこのエントリの議論では多用する．例えば図 1 を見ながら「$\v{x}_{k-1}$ と $\v{v}_{k-1}$ は独立かどうか」を考えると，$\v{x}_{k-1}$ は青字の変数 $\v{x}_0, \v{v}_0, \ldots, \v{v}_{k-2}$ に依存していて，$\v{v}_{k-1}$ はそれらと独立だから，独立であると結論できる．

多次元正規分布 (多変量正規分布)

• 1次元正規分布 (ガウス分布) の密度関数は $p(x) = \frac{1}{\sqrt{2\pi \sigma^2}} \exp\{-\frac{1}{2} \frac{(x - \mu)^2}{\sigma^2} \}$ で与えられる．$\mu$ は平均．$\sigma^2$ は分散．
• $\v{x}$ を $n$ 次元ベクトルに拡張するには，$\v{\mu}$ をベクトルに，$\sigma^2$ を共分散行列 $\m{P}$ にする．exp の中身は二次形式に置き換わる: $p(\v{x}) = \frac{1}{\sqrt{(2\pi)^n |\m{P}|}} \exp\{-\frac{1}{2} (\v{x} - \v{\mu})\trans \m{P}\inv (\v{x} - \v{\mu})\}$
  • 共分散行列 $\m{P}$ は対称なので，その逆行列 $\m{P}\inv$ も対称行列になる (したがって確かに exp の中身は二次形式である) ことに注意しておく．
  • exp の前の係数部分は，積分したときに 1 になるように定められている．なんかゴチャゴチャしているが，このエントリでは重要ではないので気にしなくてよい．
• このエントリで多用する性質を紹介しておく: 正規分布に従う独立な確率ベクトル $\v{x}$ と $\v{y}$ が与えられた場合，任意の確定値行列 $\m{A}, \m{B}$ に対して，$\m{A}\v{x} + \m{B}\v{y}$ も正規分布に従う．
  • スカラ確率変数の場合 ($x$ と $y$ が独立に正規分布に従うなら $ax + by$ も正規分布に従う) はよく知られていることと思う (定数倍は変数置換で，和はたたみこみで理解できる)．これのベクトル版だと理解しておこう (ベクトルに拡張できることは決して自明ではないのだけど)．

最適推定 = 条件付き期待値

カルマンフィルタの問題からいったん離れて，何か適当な観測モデルに基づいて観測値 $\v{y}$ から未知状態 $\v{x}$ を推測する統計的推定問題を考える．

このとき，観測値 $\v{y}$ が与えられたときの $\v{x}$ の条件付き確率密度関数
$$
\begin{align}
p(\v{x} \given \v{y}) &\defeq \frac{p(\v{x}, \v{y})}{p(\v{y})}
\end{align}
$$ が評価できれば，その代表値を求めることで推定値 (点推定) が得られる．

代表値の取り方はいろいろあるが，もし $p(\v{x} \given \v{y})$ が正規分布に従うとわかっているならば，その分布の中心，つまり期待値を取るのがベストであることに疑いの余地はない．このように取った推定値のことを条件付き期待値と呼ぶ．
\begin{align}
\E[\v{x} \given \v{y}] &\defeq \int \v{x} p(\v{x} \given \v{y}) d\v{x}
\label{eq:fuse:cm}
\end{align}

今回の導出では，$p(\v{x} \given \v{y})$ が正規分布に従うことを常に確認しながら条件付き期待値を評価していくことにする．

正規分布とは限らない場合，代表値の取り方は自明ではなくて，例えば最頻値を取るとか中央値を取るとかいろいろな方策が考えられ，どれを選ぶのが良いとは一概に言えないように思える．ところがそのようなあらゆる推定値の作り方の中で，二乗誤差の期待値を最小にする意味で「最適」なのは条件付き期待値であると示すことができる．分布の形に関わらずである．
実際に確認してみよう．与えられた $\v{y}$ から計算される推定値を $\hat{\v{x}} = \hat{\v{x}}(\v{y})$ と書くことにして，まず二乗誤差 $\| \hat{\v{x}} - \v{x} \|^2$ の $\v{y}$ による条件付き期待値を計算してみる．$\hat{\v{x}}$ は $\v{y}$ が与えられた条件下では $\v{x}$ に依らない定数であることに注意すると， $$ \begin{align} \E[\| \hat{\v{x}} - \v{x} \|^2 \given \v{y}] &= \E[ \hat{\v{x}}\trans\hat{\v{x}} - 2 \hat{\v{x}}\trans \v{x} + \v{x}\trans \v{x} \given \v{y}]\\ &= \hat{\v{x}}\trans\hat{\v{x}} - 2 \hat{\v{x}}\trans \E[\v{x} \given \v{y}] + \E[\v{x}\trans \v{x} \given \v{y}]\\ &= \| \hat{\v{x}} - \E[\v{x} \given \v{y}] \|^2 + \cdots \end{align} $$ ただし $\cdots$ のところは $\hat{\v{x}}$ に依らない部分である．これより，与えられた $\v{y}$ に対して $\hat{\v{x}} = \E[\v{x} \given \v{y}]$ と取ることで条件付き二乗誤差が最小になることがわかる．
次に，$\v{y}$ を固定しない場合の二乗誤差の期待値を考える． $$ \begin{align} \E[\| \hat{\v{x}} - \v{x} \|^2 ] &= \iint \| \hat{\v{x}} - \v{x} \|^2 p(\v{x}, \v{y}) d\v{x} d\v{y}\\ &= \iint \| \hat{\v{x}} - \v{x} \|^2 p(\v{x} \given \v{y}) p(\v{y}) d\v{x} d\v{y}\\ &= \int \E[\| \hat{\v{x}} - \v{x} \|^2 \given \v{y}] p(\v{y}) d\v{y} \end{align} $$ と展開できるわけだが，この最後の $\v{y}$ で積分される被積分関数は，各点 $\v{y}$ において $\hat{\v{x}} = \E[\v{x} \given \v{y}]$ のときに最小かつ非負なのだから，この積分も当然そう取るときに最小になる．
この意味で，条件付き期待値のことを最小分散推定値あるいは最適推定値などと呼ぶことがある．

尤度関数

以上のように，推定値を求めるには $p(\v{x} \given \v{y})$ が欲しいのだが，一般に直接これをモデル化するのは難しい．というのは，観測値 $\v{y}$ というのは状態 $\v{x}$ から何らかの物理的プロセスまたは計算処理などを経て生成されるのが普通であって，$p(\v{x} \given \v{y})$ はその逆問題を解くことに相当するからである．言い換えると，$p(\v{y} \given \v{x})$ ならばモデル化しやすい場合が圧倒的に多い．
ここで登場するのがベイズの定理 $$ \begin{align} p(\v{x} \given \v{y}) &= \frac{p(\v{y}, \v{x}) }{p(\v{y})} = \frac{p(\v{y} \given \v{x}) p(\v{x})}{p(\v{y})} \propto p(\v{y} \given \v{x}) p(\v{x}) \label{eq:fuse:bayes} \end{align} $$ である．これを使うと $p(\v{x} \given \v{y})$ を $p(\v{y} \given \v{x})$ を使って表すことができる．

• 分母の $p(\v{y})$ は $\v{x}$ に依らない値である．何か具体的な $\v{y}$ が得られたとして，密度分布の形状を実質的に定めるのは分子の $p(\v{y} \given \v{x}) p(\v{x})$ であり，分母は $p(\v{x} \given \v{y})$ が確率密度関数の要件 (積分したら 1 になる) を満たすための規格化定数としてはたらくだけである．規格化定数の具体的な値は計算上必要ないことも多く，そういう場合は「比例する ($\propto$)」とだけ考えていちいち計算しないという戦略は有用である．
• $p(\v{x})$ と $p(\v{x} \given \v{y})$ をそれぞれ事前分布の確率密度関数，事後分布の確率密度関数と呼ぶ．長いので事前確率密度，事後確率密度などとも呼ぶ．
• この公式を使うとき，$p(\v{y} \given \v{x})$ の部分は $\v{y}$ を固定して $\v{x}$ の関数として見ることになる．このように $\v{x}$ の関数として見るとき，$p(\v{y} \given \v{x})$ を尤度関数と呼ぶ．

カルマンフィルタ問題に少しだけ近寄った観測モデル
$$
\begin{align}
\v{y} &= \m{H} \v{x} + \v{w}
\end{align}
$$ を考える．まだ時系列の概念は入れない．$\v{w}$ は平均 $\v{0}$，共分散 $\m{R}$ の正規分布に従う観測雑音であり，$\v{x}$ とは独立に発生するとする．

このとき $p(\v{y} \given \v{x})$ がどうなるかというと，$\v{y}$ は平均 $\m{H} \v{x}$ の周りに共分散 $\m{R}$ で広がる正規分布に従うのだから，
$$
\begin{align}
p(\v{y} \given \v{x}) &= \frac{1}{\sqrt{(2\pi)^m |\m{R}|}} \exp\{-\frac{1}{2} (\v{y} - \m{H} \v{x})\trans \m{R}\inv (\v{y} - \m{H} \v{x})\}
\label{eq:fuse:gaussian}\\
\label{eq:fuse:loglikelihood}
\end{align}
$$ となる．これを $\v{x}$ の関数として見たものがこの観測モデルの尤度関数 (観測尤度) である．

事前情報がない場合

あえて遠回りして，$\v{x}$ に関する事前情報がない場合を考える．$p(\v{x} \given \v{y}) \propto p(\v{y} \given \v{x}) p(\v{x})$ のうち $p(\v{x})$ が一様分布に近づく極限を考えるので，$p(\v{x} \given \v{y}) \propto p(\v{y} \given \v{x})$ である．だから，$p(\v{y} \given \v{x})$ が ($\v{x}$ に関して) どのような形になっているかに興味がある．

$p(\v{y} \given \v{x})$ の指数部を $-\frac{1}{2} L(\v{x})$ と書くことにして，$L(\v{x}) = (\v{y} - \m{H} \v{x})\trans \m{R}\inv (\v{y} - \m{H} \v{x})$ を $\v{x}$ に関して平方完成する．
$$
\begin{align}
L(\v{x})
&= (\v{x} - \v{b})\trans \m{H}\trans\m{R}\inv\m{H}(\v{x} - \v{b}) + c
\label{eq:fuse:linearloglikelihood}\\
\text{ただし } & \v{b} = (\m{H}\trans \m{R}\inv \m{H})\inv \m{H}\trans \m{R}\inv \v{y}
\end{align}
$$ ここで $c$ は $\v{x}$ に依らない定数である．

二次形式の平方完成は正規分布を扱う場合に頻出する計算だが，慣れていない場合は以下のように考えるとよい．まず， $$ \begin{align} L(\v{x}) &= \v{x}\trans \m{H}\trans \m{R}\inv \m{H} \v{x} - \v{x}\trans \m{H}\trans \m{R}\inv \v{y} - \v{y}\trans \m{R}\inv \m{H} \v{x} + \v{y}\trans \m{R}\inv \v{y} \end{align} $$ と展開できるが，ここで $\m{R}\inv$ は対称である (正規分布の式のここに来るのは常に対称行列である) ことに注意すると，第2項と第3項はいずれも $\v{x}$ と $\m{H}\trans \m{R}\inv \v{y}$ の内積であって同じものであることがわかる．つまりこれらをまとめて $$ \begin{align} L(\v{x}) &= \v{x}\trans \m{H}\trans \m{R}\inv \m{H} \v{x} - 2 \v{x}\trans \m{H}\trans \m{R}\inv \v{y} + \v{y}\trans \m{R}\inv \v{y} \end{align} $$ と書ける．
平方完成するというのは，これを $(\v{x} - \v{b})\trans \m{A} (\v{x} - \v{b}) + c $ の形に持ち込むことであるが，まず $\v{x}$ に関する 2 次の項を見比べると $\m{A} = \m{H}\trans\m{R}\inv\m{H}$ に定まる．$\m{A}$ が対称行列になることに注意する． ($\m{A}\trans = \m{H}\trans (\m{R}\inv)\trans \m{H} = \m{A}$)
同様に $\v{x}$ に関する 1 次の項を比較すると $\m{A} \v{b} = \m{H}\trans \m{R}\inv \v{y}$ であり，これを解いて $\v{b} = \m{A}\inv \m{H}\trans \m{R}\inv \v{y}$ が得られる． 残りの $\v{x}$ に依存しない項については，我々の目的には具体的な形は必要ないので気にしない．

これを $p(\v{x} \given \v{y}) \propto p(\v{y} \given \v{x})$ に代入し，指数関数の積法則を使うと，定数項 $c$ の部分は規格化定数の一部として吸収できてしまう．いちいち計算しなくてよかったですね．
$$
\begin{align}
p(\v{x} \given \v{y})
&\propto \exp\{-\frac{1}{2} \left[ (\v{x} - \v{b})\trans \m{H}\trans\m{R}\inv\m{H}(\v{x} - \v{b}) + c \right]\}\\
&= \exp\left\{-\frac{1}{2} (\v{x} - \v{b})\trans \m{H}\trans\m{R}\inv\m{H}(\v{x} - \v{b})\right\} \exp{\left\{ -\frac{c}{2} \right\}}\\
&\propto \exp\left\{-\frac{1}{2} (\v{x} - \v{b})\trans \m{H}\trans\m{R}\inv\m{H}(\v{x} - \v{b})\right\}
\label{eq:fuse:posterior}
\end{align}
$$ この式の形から，$p(\v{x} \given \v{y})$ は平均 $\v{b}$，共分散 $(\m{H}\trans \m{R}\inv \m{H})\inv$ の正規分布に従うことがわかる．

以上から最適推定 $\hat{\v{x}} = \E[\v{x} \given \v{y}]$ は $\v{b}$ によって与えられることがわかる．
\begin{align}
\hat{\v{x}} &= (\m{H}\trans \m{R}\inv \m{H})\inv \m{H}\trans \m{R}\inv \v{y}
\label{eq:fuse:estimate}
\end{align}

$\m{R} = \m{I}$ の場合は $\hat{\v{x}} = (\m{H}\trans \m{H})\inv \m{H}\trans \v{y}$ になるわけだが，この式を見て「あー，$\v{y} = \m{H} \v{x}$ の最小二乗解だな」とピンと来る人も多いのではないかと思う．
実際，これは正規分布の中心，つまり確率密度関数 ($\propto \exp\{ -\frac{1}{2} L(\v{x})\}$) が最大になるような $\v{x}$ だったわけで，すなわち $L(\v{x})$ を最小とするような $\v{x}$ である．$\m{R} = \m{I}$ のときは $L(\v{x}) = (\v{y} - \m{H} \v{x})\trans (\v{y} - \m{H} \v{x}) = \| \v{y} - \m{H} \v{x} \|^2$ なので，確かに最小二乗解を求めていることになる．
$\m{R} \neq \m{I}$ の場合はこれを一般化したもので，重み付き最小二乗法と呼ばれる．

このようにして得られる推定値がどのくらい良いものであるかを評価できるようにしておくことは有用である．そのような評価は推定誤差共分散 (あるいは単に誤差共分散) と呼ばれるものによって表せる．推定誤差を $\v{e} \defeq \hat{\v{x}} - \v{x}$ として，推定誤差共分散は $\E[\v{e}\v{e}\trans]$ で定義する．

推定誤差は，$\v{e} = \hat{\v{x}} - \v{x}$ に $\v{y} = \m{H} \v{x} + \v{w}$ を代入して
\begin{align}
\v{e}
&= (\m{H}\trans \m{R}\inv \m{H})\inv \m{H}\trans \m{R}\inv (\m{H} \v{x} + \v{w}) - \v{x}\\
&= (\m{H}\trans \m{R}\inv \m{H})\inv \m{H}\trans \m{R}\inv \v{w}
\label{eq:fuse:linearerrvec}
\end{align}

推定誤差共分散は，$\m{W} \defeq \m{H}\trans \m{R}\inv \m{H}$ が対称行列であることに注意しながら計算して，$$
\begin{align}
\E[\v{e}\v{e}\trans]
&= \E[ \m{W}\inv \m{H}\trans \m{R}\inv \v{w} \v{w}\trans
(\m{R}\inv)\trans \m{H} (\m{W}\inv)\trans]\\
&= \m{W}\inv \m{H}\trans \m{R}\inv \E[\v{w} \v{w}\trans]
(\m{R}\inv)\trans \m{H} (\m{W}\inv)\trans\\
&= \m{W}\inv \m{H}\trans \m{R}\inv \m{R} \m{R}\inv \m{H} \m{W}\inv\\
&= (\m{H}\trans \m{R}\inv \m{H})\inv
\label{eq:fuse:linearerrcov}
\end{align}
$$ となる．

この誤差共分散は式 (\ref{eq:fuse:posterior}) からわかる $p(\v{x} \given \v{y})$ の共分散と一致している．この一致はただの偶然ではない．
まず，両者は本来は別物だと認識することは重要である．共分散は期待値まわりのばらつきを評価するものである．一方，誤差共分散は推定値まわりのばらつきを評価している (たぶんこれを「共分散」と呼ぶのは本来は適切でないが慣習的にこう呼ばれる)．別物なのだが，実は我々の問題設定では両者は一致することが示される．以下，細かい話なので興味のある人だけ読んでもらえればよい．

• 式 (\ref{eq:fuse:posterior}) の $(\m{H}\trans \m{R}\inv \m{H})\inv$ は事後確率分布の共分散 $\E[(\v{x} - \E[\v{x} \given \v{y}])(\v{x} - \E[\v{x} \given \v{y}])\trans \given \v{y}]$ である．今回の議論で我々は推定値 $\hat{\v{x}}$ として条件付き期待値 $\E[\v{x} \given \v{y}]$ を採用したので，これは $\E[(\hat{\v{x}} - \v{x})(\hat{\v{x}} - \v{x})\trans \given \v{y}]$ すなわち「条件付き誤差共分散」とでも呼ぶべきものに一致する．
• 条件付き誤差共分散は一般には $\v{y}$ の関数である．これの $\v{y}$ に関する期待値を取ったものが，無条件誤差共分散 $\E[\v{e}\v{e}\trans]$ である．($\int \E[\v{e}\v{e}\trans \given \v{y}] p(\v{y}) d\v{y} = \iint \v{e}\v{e}\trans p(\v{x}, \v{y}) d\v{x} d\v{y}$)
• ところで，式 (\ref{eq:fuse:linearerrcov}) の「推定誤差共分散」は具体的な観測 $\v{y}$ を得た際の誤差について考えたものなので，本来の意味としては「条件付き推定誤差共分散」である．ところが，我々の観測モデルが線形であるがゆえに，推定誤差は式 (\ref{eq:fuse:linearerrvec}) のように観測誤差の線形変換として得られ，その結果「条件付き推定誤差共分散」は式 (\ref{eq:fuse:linearerrcov}) のように $\v{y}$ に依存しない定数となった．よって $\v{y}$ に関して期待値をとっても同じ値である．そのためこの場合は「条件付き」と「無条件」の誤差共分散を区別せずに用いることが正当化される．
• 結局，式 (\ref{eq:fuse:posterior}) と式 (\ref{eq:fuse:linearerrcov}) の「$(\m{H}\trans \m{R}\inv \m{H})\inv$」が一致するのは，推定値として条件付き期待値を採用し，観測モデルとして線形方程式を採用した我々の選択によって結果的に起きたことであるといえる．

複数の独立な観測に分けられる場合

ちょっと前の注釈で，最適推定を重み付き最小二乗法によって解釈した．もう少し深い理解を得るためには，観測モデルを複数の独立なモデルに分けてみる (複数のセンサで観測して，それぞれに独立な雑音が乗ると考える) のが有用である．ついでに，これは事前情報がある場合の推定を考えるための伏線にもなっている．

複数の観測モデルに分けて考える．
$$
\begin{align}
\v{y}_i &= \m{H}_i \v{x} + \v{w}_i \quad (i = 1, 2, \ldots, N)
\label{eq:fuse:multisensors}
\end{align}
$$ 平均が 0 なのは分けても変わらないので $\E[\v{w}_i] = 0$ である．ここで $\v{w}_i$ は互いに独立であるという条件を設けよう．独立ならば無相関なので $\E[\v{w}_i \v{w}_j\trans] = \m{O} \,\,(i \neq j)$ である．$\E[\v{w}_i \v{w}_i\trans] = \m{R}_i$ と書くことにする．

分解前の観測モデルとの関係は
$$
\begin{align}
\v{y} &= \begin{pmatrix}
\v{y}_1\\
\v{y}_2\\
\vdots\\
\v{y}_N
\end{pmatrix}, \,\,
\m{H} = \begin{pmatrix}
\m{H}_1\\
\m{H}_2\\
\vdots\\
\m{H}_N
\end{pmatrix}, \,\,
\v{w} = \begin{pmatrix}
\v{w}_1\\
\v{w}_2\\
\vdots\\
\v{w}_N
\end{pmatrix}, \,\,
\m{R}\inv = \begin{pmatrix}
\m{R}_1\inv & & & \m{O} \\
& \m{R}_2\inv & & \\
& & \ddots & \\
\m{O} & & & \m{R}_N
\end{pmatrix}
\end{align}
$$ である．式 (\ref{eq:fuse:estimate}), (\ref{eq:fuse:linearerrvec}), (\ref{eq:fuse:linearerrcov}) に代入すると，$\m{R}\inv$ がブロック対角になっているおかげでさくさく計算できて，最適推定 $\hat{\v{x}} = \E[\v{x} \given \v{y}]$，推定誤差 $\v{e} = \hat{\v{x}} - \v{x}$，推定誤差共分散 $\E[\v{e}\v{e}\trans]$ は
$$
\begin{align}
\hat{\v{x}} &= (\sum_{i=1}^N \m{H}_i\trans \m{R}_i\inv \m{H}_i)\inv \sum_{i=1}^N
(\m{H}_i\trans \m{R}_i\inv \v{y}_i)
\label{eq:fuse:multiestimate}\\
\v{e} &= (\sum_{i=1}^N \m{H}_i\trans \m{R}_i\inv \m{H}_i)\inv \sum_{i=1}^N
(\m{H}_i\trans \m{R}_i\inv \v{w}_i)
\label{eq:fuse:multierrvec}\\
& \E[\v{e}\v{e}\trans] = (\sum_{i=1}^N \m{H}_i\trans \m{R}_i\inv \m{H}_i)\inv
\label{eq:fuse:multicov}
\end{align}
$$ となる．

$\hat{\v{x}}$ は複数の観測値 $\v{y}_i$ の重み付き和になっているわけだが，$\v{y}$ の空間から $\v{x}$ の空間への変換と重み付き和の計算が混ざっているため一見わかりにくい．
仮に $\v{y}_i = \m{H}_i \v{x}_i$ を満たすような $\v{x}_i$ が存在すると仮定してみよう (実際には，$\m{H}_i$ は可逆とは限らないし雑音 $\v{w}_i$ も乗っているので，そんなものは存在するとは限らないし，存在しても唯一とは限らないのだが)．すると $$ \begin{align} \hat{\v{x}} &= (\sum_{i=1}^N \m{H}_i\trans \m{R}_i\inv \m{H}_i)\inv \sum_{i=1}^N (\m{H}_i\trans \m{R}_i\inv \m{H}_i \v{x}_i) \end{align} $$ のようになる．つまり各 $\v{x}_i$ に重み $\m{H}_i\trans \m{R}_i\inv \m{H}_i$ をかけてから足し合わせて，全重み $\sum_{i=1}^N \m{H}_i\trans \m{R}_i\inv \m{H}_i$ で規格化したことになる．このことを，$\v{x}_i$ の存在を仮定せずに表したのが上記の最適推定であるということになる．
重み $\m{H}_i\trans \m{R}_i\inv \m{H}_i$ は，前節の結果からわかるように，観測 $\v{y}_i$ だけから推定を行った場合の推定誤差共分散の逆行列に相当する．つまり推定誤差が小さくなる観測値に大きな重みをつけるものであると解釈できる．

事前情報がある場合

再度，事前情報がある場合のベイズ推定の式に戻る．
$$
\begin{align}
p(\v{x} \given \v{y}) &\propto p(\v{y} \given \v{x}) p(\v{x})
\label{eq:fuse:bayes2}
\end{align}
$$ この $p(\v{x})$ が，平均 $\bar{\v{x}}$，共分散 $\m{P}$ の正規分布の密度関数である場合を考える．

…のだが，ちょっと発想を変えて前節の結果を利用することを考える．この事前情報が得られたことは，平均 $\v{0}$，共分散 $\m{P}$ の正規分布に従う $\v{\epsilon}$ を観測雑音とする仮想的なセンサ
$$
\begin{align}
\v{y}^\text{prior} &= \v{x} + \v{\epsilon}
\label{eq:fuse:virtualsensor}
\end{align}
$$ から観測値 $\v{y}^\text{prior} = \bar{\v{x}}$ が得られたことと等価である．観測雑音 $\v{w}$ は状態とは独立に発生するとしているので $\v{\epsilon}$ と $\v{w}$ は独立である．したがってさっきの複数の独立な観測に分けられる場合 ($N = 2$) の話を適用できる．

よって最適推定 $\hat{\v{x}} = \E[\v{x} \given \v{y}]$，推定誤差 $\v{e} = \hat{\v{x}} - \v{x}$，推定誤差共分散 $\E[\v{e}\v{e}\trans]$ はそれぞれ
$$
\begin{align}
\hat{\v{x}} &= \m{D}\inv (\m{P}\inv \bar{\v{x}} + \m{H}\trans \m{R}\inv \v{y})
\label{eq:fuse:mapestimate}\\
\v{e} &= \m{D}\inv (\m{P}\inv \v{\epsilon} + \m{H}\trans \m{R}\inv \v{w})
\label{eq:fuse:maperrvec}\\
\E[\v{e}\v{e}\trans] &= \m{D}\inv
\label{eq:fuse:mapcov}
\end{align}
$$ となる．ただし $\m{D} \defeq \m{P}\inv + \m{H}\trans \m{R}\inv \m{H}$ と書いた．

結局，複数の独立な観測に分かれる場合に「観測ごとの重み付き和」になったのと同様に，「事前情報と観測の重み付き和」になると理解すればよい．

前節「事前情報がない場合」をすっ飛ばして，いきなり計算することもできる．簡潔さのみを重視するならその方が早いはずである．
式 (\ref{eq:fuse:bayes2}) の右辺 $p(\v{y} \given \v{x}) p(\v{x})$ は，指数関数の積法則で 1 つの指数関数にまとめられる． \begin{align} p(\v{y} \given \v{x}) p(\v{x}) &\propto \exp\left\{ -\frac{1}{2} (\v{y} - \m{H} \v{x})\trans \m{R}\inv (\v{y} - \m{H} \v{x})\right\} \exp\left\{ -\frac{1}{2} (\v{x} - \bar{\v{x}})\trans \m{P}\inv (\v{x} - \bar{\v{x}})\right\} \\ &= \exp\left\{ -\frac{1}{2} [ (\v{y} - \m{H} \v{x})\trans \m{R}\inv (\v{y} - \m{H} \v{x}) + (\v{x} - \bar{\v{x}})\trans \m{P}\inv (\v{x} - \bar{\v{x}})]\right\} \end{align} 指数部を $\v{x}$ について整理すると $$ \begin{align} p(\v{y} \given \v{x}) p(\v{x}) &\propto \exp\left\{ -\frac{1}{2} [ \v{x}\trans (\m{H}\trans \m{R}\inv \m{H} + \m{P}\inv) - 2 \v{x}\trans (\m{H}\trans \m{R}\inv \v{y} + \m{P}\inv \bar{\v{x}}) + \cdots]\right\} \end{align} $$ ただし $\cdots$ は $\v{x}$ に依らない定数項の部分を表す．これを平方完成すれば，事後確率は正規分布に従うことがわかり最適推定値 $\hat{\v{x}}$ が得られる．$\bar{\v{x}} = \v{x} + \v{\epsilon}$ と $\v{y} = \m{H} \v{x} + \v{w}$ から推定誤差 $\v{e}$ が，$\v{\epsilon}$ と $\v{w}$ の独立性から推定誤差共分散 $\E[\v{e}\v{e}\trans]$ が得られ，本文中で得られたものと同じ結果になる．

カルマンフィルタ問題の解

さて本丸に進もう．目的は，最適推定 $\E[\v{x}_k \given \v{y}_1, \v{y}_2, \cdots, \v{y}_k]$ を求めることである．長いので $\v{y}_1, \v{y}_2, \cdots, \v{y}_k$ をまとめて $\Ys_k$ と書くことにする．つまり $\E[\v{x}_k \given \Ys_k]$ を求めたい．

さらに略記して $\hat{\v{x}}_{k \given l} \defeq \E[\v{x}_k \given \Ys_l]$ と書く．これは「時刻 $l$ までの観測を得た際の $\v{x}_k$ の最適推定値」である (定義上，$k$ と $l$ は任意の整数でよいが，以下では $l = k$ と $l = k - 1$ の場合だけが現れる)．その推定誤差を $\v{e}_{k \given l} \defeq \hat{\v{x}}_{k \given l} - \v{x}_k$，推定誤差共分散を $\m{P}_{k \given l} \defeq \E[\v{e}_{k \given l} \v{e}_{k \given l}\trans]$ と書くことにする．

以下では，

• 時刻 $k-1$ の最適推定 $\hat{\v{x}}_{k-1 \given k-1}$ と推定誤差共分散 $\m{P}_{k-1 \given k-1}$ が既知であり，推定誤差 $\v{e}_{k-1 \given k-1}$ が平均 $\v{0}$ の正規分布に従う

ことを仮定すると，

• 時刻 $k$ の最適推定 $\hat{\v{x}}_{k \given k}$ と推定誤差共分散 $\m{P}_{k \given k}$ が計算でき，推定誤差 $\v{e}_{k \given k}$ は平均 $\v{0}$ の正規分布に従う

ことを示す．初期時刻について $\hat{\v{x}}_{0 \given 0} = \v{\mu}_0$ と $\m{P}_{0 \given 0} = \m{P}_0$ は既知であり $\v{e}_0 = \v{\mu}_0 - \v{x}_0$ は平均 $\v{0}$ の正規分布に従うので，帰納法により任意の $k$ について最適推定が得られることが示される．

時間更新

まず $\hat{\v{x}}_{k \given k-1}$ を求める．これは，プロセスモデルに従って1時刻分の予測を行うことであって，時間更新と呼ばれる．実のところ割と簡単で，条件付き期待値の計算を普通に行うだけでよい．
$$
\begin{align}
\hat{\v{x}}_{k \given k-1}
&= \E[ \m{x}_{k} \given \Ys_{k-1}]\\
&= \E[ \m{F}_{k-1} \v{x}_{k-1} + \v{v}_{k-1} \given \Ys_{k-1}]\\
&= \m{F}_{k-1} \E[ \v{x}_{k-1} \given \Ys_{k-1}] + \E[\v{v}_{k-1} \given \Ys_{k-1}]\\
&= \m{F}_{k-1} \hat{\v{x}}_{k-1 \given k-1}
\label{eq:fuse:timeupdate}
\end{align}
$$ ここで，図1 から $\v{v}_{k-1}$ は $\Ys_{k-1}$ とは独立であり，したがって $\E[\v{v}_{k-1} \given \Ys_{k-1}] = \E[\v{v}_{k-1}] = \v{0}$ であることを用いた．

推定誤差は
\begin{align}
\v{e}_{k \given k-1}
&= \m{F}_{k-1}\hat{\v{x}}_{k-1 \given k-1} - (\m{F}\v{x}_{k-1} + \v{v}_{k-1}) \nonumber\\
&= \m{F}_{k-1}\v{e}_{k-1 \given k-1} - \v{v}_{k-1}
\label{eq:fuse:timeupdateerr}
\end{align}

ここで $\v{v}_{k-1}$ は $\v{e}_{k-1 \given k-1} = \hat{\v{x}}_{k-1 \given k-1} - \v{x}_{k-1}$ とは独立に発生する．なぜならば，図1 を見れば $\v{v}_{k-1}$ と $\v{x}_{k-1}$ が独立であることがわかるし，$\hat{\v{x}}_{k-1 \given k-1} = \E[\v{x}_{k-1} \given \Ys_{k-1}]$ は $\v{y}_1, \cdots, \v{y}_{k-1}$ から計算されるものなので，やはり 図1 から $\v{v}_{k-1}$ とは独立だとわかるからである．

正規分布に従う独立な確率ベクトルの重み付き和は正規分布に従うことから，$\v{e}_{k \given k-1}$ はやはり正規分布に従う．$\v{e}_{k-1 \given k-1}$ も $\v{v}_{k-1}$ も平均 $\v{0}$ だから，$\v{e}_{k \given k-1}$ の平均もやはり $\v{0}$ である．

推定誤差共分散は
$$
\begin{align}
\m{P}_{k \given k-1}
&= \E[ (\m{F}_{k-1}\v{e}_{k-1 \given k-1} - \v{v}_{k-1}) (\m{F}_{k-1}\v{e}_{k-1 \given k-1} - \v{v}_{k-1})\trans]
\end{align}
$$ を計算することで得られる．展開すると $\E[\v{e}_{k-1 \given k-1}\v{e}_{k-1 \given k-1}\trans]$ の項，$\E[\v{v}_{k-1}\v{v}_{k-1}\trans]$ の項，$\E[\v{e}_{k-1 \given k-1}\v{v}_{k-1}\trans]$ の項が出てくるが，$\v{e}_{k-1 \given k-1}$ と $\v{v}_{k-1}$ が独立なので $\E[\v{e}_{k-1 \given k-1}\v{v}_{k-1}\trans] = \m{O}$ である．よって
\begin{align}
\m{P}_{k \given k-1}
&= \m{F}_{k-1} \E[\v{e}_{k-1 \given k-1}\v{e}_{k-1 \given k-1}\trans ] \m{F}_{k-1}\trans + \E[\v{v}_{k-1} \v{v}_{k-1}\trans]\\
&= \m{F}_{k-1} \m{P}_{k-1 \given k-1} \m{F}_{k-1}\trans + \m{Q}_{k-1}
\label{eq:fuse:timeupdatecov}
\end{align}

観測更新

次に $\hat{\v{x}}_{k \given k}$ を求める．これは，時間更新の結果を事前情報として，独立な観測雑音の乗った観測値 $\v{y}_k$ を得たときの最適推定を求めることに他ならない．

…という点は直観的には当たり前な気もするのだが，念のため真面目に確認しておく．条件付き確率密度の定義を繰り返し適用することで， \begin{align} p(\v{x}_k \given \Ys_k) &= \frac{p(\v{x}_k, \Ys_k)}{p(\Ys_k)} \\ &= \frac{p(\v{x}_k, \Ys_{k-1}, \v{y}_k)}{p(\Ys_k)} \\ &= \frac{p(\v{y}_k \given \v{x}_k, \Ys_{k-1}) p(\v{x}_k, \Ys_{k-1})}{p(\Ys_k)} \\ &= \frac{p(\v{y}_k \given \v{x}_k, \Ys_{k-1}) p(\v{x}_k \given \Ys_{k-1}) p(\Ys_{k-1})}{p(\Ys_k)} \end{align} ここで $p(\v{y}_k \given \v{x}_k, \Ys_{k-1}) = p(\v{y}_k \given \v{x}_k)$ である．なぜならば $\v{y}_k = H_k \v{x}_k + \v{w}_k$ であり，$\v{x}_k$ の条件付けは $\v{x}_k$ だけで十分だし，図1 より $\v{w}_k$ は $\v{x}_k$ からも $\Ys_{k-1}$ からも独立だからである．よって \begin{align} p(\v{x}_k \given \Ys_k) &\propto p(\v{y}_k \given \v{x}_k) p(\v{x}_k \given \Ys_{k-1}) \end{align} これは式 (\ref{eq:fuse:bayes2}) と全く同じ形式をしている．だから式 (\ref{eq:fuse:virtualsensor}) から (\ref{eq:fuse:mapcov}) に至るまでの議論において，$p(\v{x} \given \v{y}) = p(\v{x}_k \given \Ys_k)$, $p(\v{y} \given \v{x}) = p(\v{y}_k \given \v{x}_k)$, $p(\v{x}) = p(\v{x}_k \given \Ys_{k-1})$ に相当する読み替えを行うことで，$p(\v{x}_k \given \Ys_k)$ による条件付き期待値，すなわち $\E[\v{x}_k \given \Ys_k] = \hat{\v{x}}_{k \given k}$ とその推定誤差，推定誤差共分散を求めることができる．

事前情報は，$\v{y}_k^\text{prior} = \v{x}_k + \v{e}_{k \given k-1}$ という仮想的な観測モデルから観測値 $\v{y}_k^\text{prior} = \hat{\v{x}}_{k \given k-1}$ が得られたということである．これを実際の観測 $\v{y}_k = \m{H}_k \v{x}_k + \v{w}_k$ と合わせて最適推定を求める．この処理を観測更新と呼ぶ．

仮想的な観測誤差 $\v{e}_{k \given k-1}$ は平均が $\v{0}$ で共分散が $\m{P}_{k \given k-1}$ である．実観測の誤差についても平均が $\v{0}$ で共分散が $\m{R}_k$ と知っている．さらに，毎度おなじみ図1を参照すると，$\v{w}_k$ は $\v{x}_k$ からも $\hat{\v{x}}_{k \given k-1} = \E[\v{x}_k \given \Ys_{k-1}]$ からも独立であるとわかるので，$\v{w}_k$ と $\v{e}_{k \given k-1}$ は独立である．

以上から，式 (\ref{eq:fuse:mapestimate}), (\ref{eq:fuse:maperrvec}), (\ref{eq:fuse:mapcov}) の結果をそのまま適用できる．
\begin{align}
\hat{\v{x}}_{k \given k}
&= \m{D}_k\inv
(\m{P}_{k \given k-1}\inv \hat{\v{x}}_{k \given k-1} + \m{H}_k\trans
\m{R}_k\inv \v{y}_k)
\label{eq:fuse:measureupdate0}\\
\m{P}_{k \given k} &= \m{D}_k\inv
\label{eq:fuse:measureupdatecov0}
\end{align}

ただし $\m{D}_k \defeq \m{P}_{k \given k-1}\inv + \m{H}_k\trans \m{R}_k\inv\m{H}_k$ と書いた．

推定誤差
$$
\begin{align}
\v{e}_{k \given k} &= \m{D}_k\inv (\m{P}_{k \given k-1}\inv \v{e}_{k \given k-1} + \m{H}_k\trans \m{R}_k\inv \v{w}_k)
\label{eq:fuse:measureupdateerr0}
\end{align}
$$ は，$\v{e}_{k \given k-1}$ と $\v{w}_k$ が独立に平均 $\v{0}$ の正規分布に従うので，やはり平均 $\v{0}$ の正規分布に従う．これにより帰納法による証明が完了した．

まとめ

というわけで，カルマンフィルタの計算は以下のようにまとめられる．まず
$$
\begin{align}
\hat{\v{x}}_{0 \given 0} &= \v{\mu}_0\\
\m{P}_{0 \given 0} &= \m{P}_0
\end{align}
$$ で初期化した後，以下を $k = 1, 2, \ldots$ について繰り返す．

時刻 $k$ の時間更新:
\begin{align}
\hat{\v{x}}_{k \given k-1} &= \m{F}_{k-1} \hat{\v{x}}_{k-1 \given k-1}\\
\m{P}_{k \given k-1} &= \m{F}_{k-1} \m{P}_{k-1 \given k-1} \m{F}_{k-1}\trans + \m{Q}_{k-1}
\end{align}

時刻 $k$ の観測更新:
\begin{align}
\hat{\v{x}}_{k \given k} &=
(\m{P}_{k \given k-1}\inv + \m{H}_k\trans \m{R}_k\inv\m{H}_k)\inv
(\m{P}_{k \given k-1}\inv \hat{\v{x}}_{k \given k-1} + \m{H}_k\trans
\m{R}_k\inv \v{y}_k)\\
\m{P}_{k \given k} &= (\m{P}_{k \given k-1}\inv + \m{H}_k\trans \m{R}_k\inv\m{H}_k)\inv
\end{align}

他のいろいろな形式

これで終わってもよいのだが，観測更新の式を見て「思てたんと違う!」という人も多いのではないかと思う．この式は「時間更新で得られた予測値と観測値の重み付き和」になっていて，これはこれで解釈しやすい式だと思うのだが，世間ではむしろ
$$
\begin{align}
\hat{\v{x}}_{k \given k} &= \hat{\v{x}}_{k \given k-1} + \text{(補正項)}\\
\m{P}_{k \given k} &= \m{P}_{k \given k-1} + \text{(補正項)}\\
\end{align}
$$ の形で表すことの方が多い．

まあ要するに $\hat{\v{x}}_{k \given k-1}$ を 1 個くくり出すことを考えればよい．再度 $\m{D}_k \defeq \m{P}_{k \given k-1}\inv + \m{H}_k\trans \m{R}_k\inv\m{H}_k$ と書くことにして，
\begin{align}
\hat{\v{x}}_{k \given k}
&= \hat{\v{x}}_{k \given k-1} - \m{D}_k\inv \m{D}_k \hat{\v{x}}_{k \given k-1}
+ \m{D}_k\inv \m{P}_{k \given k-1}\inv \hat{\v{x}}_{k \given k-1}
+ \m{D}_k\inv \m{H}_k\trans \m{R}_k\inv \v{y}_k \\
&= \hat{\v{x}}_{k \given k-1} + \m{D}_k\inv
(\m{H}_k\trans \m{R}_k\inv \v{y}_k - \m{H}_k\trans \m{R}_k\inv\m{H}_k \hat{\v{x}}_{k \given k-1})\\
&= \hat{\v{x}}_{k \given k-1} + \m{D}_k\inv \m{H}_k\trans \m{R}_k\inv
(\v{y}_k- \m{H}_k \hat{\v{x}}_{k \given k-1})
\end{align}

同様に，
\begin{align}
\m{P}_{k \given k}
&= \m{P}_{k \given k-1} - \m{D}_k\inv \m{D}_k \m{P}_{k \given k-1}
+ \m{D}_k\inv\\
&= \m{P}_{k \given k-1} - \m{D}_k\inv (\m{I} + \m{H}_k\trans \m{R}_k\inv \m{H}_k \m{P}_{k \given k-1})
+ \m{D}_k\inv\\
&= \m{P}_{k \given k-1} - \m{D}_k\inv \m{H}_k\trans \m{R}_k\inv\m{H}_k \m{P}_{k \given k-1}
\end{align}

これらに共通して現れる係数を
$$
\begin{align}
\m{K}_k &\defeq \m{D}_k\inv \m{H}_k\trans\m{R}_k\inv\\
&= (\m{P}_{k \given k-1}\inv + \m{H}_k\trans \m{R}_k\inv\m{H}_k)\inv \m{H}_k\trans\m{R}_k\inv
\label{eq:fuse:kalmangain}
\end{align}
$$ と書くと，おそらく最もポピュラーな形のカルマンフィルタの式が得られる．
\begin{align}
\hat{\v{x}}_{k \given k}
&= \hat{\v{x}}_{k \given k-1} + \m{K}_k(\v{y}_k - \m{H}_k \hat{\v{x}}_{k \given k-1})\\
\m{P}_{k \given k}
&= \m{P}_{k \given k-1} - \m{K}_k \m{H}_k \m{P}_{k \given k-1}\\
&= (\m{I} - \m{K}_k \m{H}_k) \m{P}_{k \given k-1}\\
\end{align}

この $\m{K}_k$ をカルマンゲインと呼ぶ．「観測値 $\v{y}_k$ と予測される観測値 $\m{H}_k \hat{\v{x}}_{k \given k-1}$ の差分にカルマンゲインをかけたもので予測値を補正する」と考えれば，これもこれで解釈しやすい式である．

カルマンゲインを元の「予測値と観測値の重み付き和」の形の式に逆輸入してやるのも趣が深い．
\begin{align}
\hat{\v{x}}_{k \given k} &= (\m{I} - \m{K}_k \m{H}_k) \hat{\v{x}}_{k \given k-1}
+ \m{K}_k \v{y}_k
\end{align}

カルマンゲインの定義に関しても「思てたんと違う!」という人がいるのではないかと思う．式 (\ref{eq:fuse:kalmangain}) の形で表すより，逆行列補題と呼ばれる公式 \begin{align} (\m{P}\inv + \m{H}\trans \m{R}\inv \m{H})\inv \m{H}\trans \m{R}\inv &= \m{P} \m{H}\trans (\m{R} + \m{H} \m{P} \m{H}\trans)\inv \label{eq:fuse:matinv} \end{align} を用いて \begin{align} \m{K}_k &= \m{P}_{k \given k-1} \m{H}_k\trans (\m{R}_k + \m{H}_k \m{P}_{k \given k-1} \m{H}_k\trans)\inv \end{align} のように表す方がポピュラーかもしれない．実際，逆行列の計算回数が少なくなるので通常はこちらを使う方が効率がよい (ただし $m \gg n$ だと逆に計算量が増えることもある)．
なお，逆行列補題が成り立つことは，両辺に左から $(\m{P}\inv + \m{H}\trans \m{R}\inv \m{H})$ を，右から $(\m{R} + \m{H} \m{P} \m{H}\trans)$ をかけることで容易に確認できる．

というわけで，結論を整理しておく．

カルマンフィルタの計算は， $$ \begin{align} \hat{\v{x}}_{0 \given 0} &= \v{\mu}_0\\ \m{P}_{0 \given 0} &= \m{P}_0 \end{align} $$ で初期化した後，以下を $k = 1, 2, \ldots$ について繰り返すことによって行われる (イコールで結ばれている式のどれを使っても同じ)．
時刻 $k$ の時間更新: \begin{align} \hat{\v{x}}_{k \given k-1} &= \m{F}_{k-1} \hat{\v{x}}_{k-1 \given k-1}\\ \m{P}_{k \given k-1} &= \m{F}_{k-1} \m{P}_{k-1 \given k-1} \m{F}_{k-1}\trans + \m{Q}_{k-1} \end{align} 時刻 $k$ の観測更新: $$ \begin{align} \hat{\v{x}}_{k \given k} &= (\m{P}_{k \given k-1}\inv + \m{H}_k\trans \m{R}_k\inv\m{H}_k)\inv (\m{P}_{k \given k-1}\inv \hat{\v{x}}_{k \given k-1} + \m{H}_k\trans \m{R}_k\inv \v{y}_k)\\ &= \hat{\v{x}}_{k \given k-1} + \m{K}_k(\v{y}_k - \m{H}_k \hat{\v{x}}_{k \given k-1})\\ &= (\m{I} - \m{K}_k \m{H}_k) \hat{\v{x}}_{k \given k-1} + \m{K}_k \v{y}_k\\ \m{P}_{k \given k} &= (\m{P}_{k \given k-1}\inv + \m{H}_k\trans \m{R}_k\inv\m{H}_k)\inv\\ &= \m{P}_{k \given k-1} - \m{K}_k \m{H}_k \m{P}_{k \given k-1}\\ &= (\m{I} - \m{K}_k \m{H}_k) \m{P}_{k \given k-1} \end{align} $$ ただし $$ \begin{align} \m{K}_k &= (\m{P}_{k \given k-1}\inv + \m{H}_k\trans \m{R}_k\inv\m{H}_k)\inv \m{H}_k\trans\m{R}_k\inv\\ &= \m{P}_{k \given k-1} \m{H}_k\trans (\m{R}_k + \m{H}_k \m{P}_{k \given k-1} \m{H}_k\trans)\inv \end{align} $$

文献ノート

• R. E. Kalman, A new approach to linear filtering and prediction problems, Transactions of the ASME, Journal of Basic Engineering, vol.82D, no.1, pp.35-45, 1960.
• Y. C. Ho and R. C. K. Lee, A Bayesian Approach to Problems in Stochastic Estimation and Control, IEEE Transactions on Automatic Control, vol.9, pp.333-339, 1964.
• A. H. Jazwinski, Stochastic Processes and Filtering Theory, Academic Press, 1970.
• 有本卓, カルマン・フィルター, 産業図書, 1977.
• B. D. O. Anderson and J. B. Moore, Optimal Filtering, Prentice-Hall, 1979.
• 片山徹, 新版 応用カルマンフィルタ, 朝倉書店, 2000.
• Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with Applications to Tracking and Navigation, John Wiley & Sons, 2001.

このエントリでは，初期値と雑音が正規分布に従うという前提のもとで最適推定を求めるものとしてカルマンフィルタを説明した．
正規分布の仮定が成り立たない場合は最適とはいえなくなるが，その場合でも，線形計算だけで実現できる状態推定器の中で最適なもの (線形最適推定) を求めると，カルマンフィルタに一致することが知られている．
カルマンフィルタはいろいろなアプローチで導出できるが，最も有名なのはいわゆる直交射影原理に基づく方法ではないかと思う．ただしこれは本来的には，(最適フィルタではなく) 線形最適推定フィルタを導出するものである．Kalman の原論文 (1960) はこのアプローチだが，さらに，同時確率が正規分布に従う無相関な確率変数は独立であることを利用して，正規分布を仮定した場合の最適性 (線形フィルタに限定せず最適であること) も示している．
直交射影原理による方法は簡潔で美しいのだけど，簡潔過ぎて不安になるというか，魔法を見せられているうちにいつの間にか答えが出てました感が強い．
直交射影原理によらない方法としてポピュラーなのは，状態 $\v{x}$ と観測 $\v{y}$ の同時分布が正規分布に従うという仮定だけに基づいて，事後分布 $p(\v{x} \given \v{y})$ の具体的な式をゴリゴリと計算して求める方法である．Anderson+ (1979)，Bar-Shalom+ (2001)，片山 (2000) など，多くの教科書がこの方法と直交射影原理の方法を扱っている．
このエントリが取ったのは，事後分布 $p(\v{x} \given \v{y})$ を求めるときのゴリゴリの計算が幾分楽になるように，状態と観測の間の関係を $\v{y} = \m{H} \v{x} + \v{w}$ の形に最初から限定してしまうアプローチで，Jazwinski (1970)，有本 (1977) などがこの方法によっている．Ho+ (1964) あたりがこの路線の端緒なのかなと思う．これらの文献では，このエントリが「規格化定数」としてバッサリ省略した部分についてもきっちり計算している．
このエントリでは，$p(\v{x} \given \v{y})$ を一気に求めてしまうのではなく，観測が複数の独立なものに分けられる場合を考えたり，事前情報も仮想的な観測の一種として考えたりという遠回りをした．考え方としては自然なものだと思っているし，初学者への説明に適していると思っているけど，カルマンフィルタの導出を目的としてこういうことをしている例はどうも見たことがない．冗長なので普通はやらないということなんだろうと思う．

リンク