Kalmanフィルタの導出

導出の概要

本記事では、Kalmanフィルタの各式を数学的に導出する。導出の方針は推定誤差パワーの期待値 $E\left[\|\boldsymbol{x}_k-\hat{\boldsymbol{x}}_{k|k}\|^2\right]$ を最小化することである。

Kalmanフィルタの基本的な設定については概要ページを参照。

前提となるモデル

以下の状態空間モデルを仮定する。

\begin{align} \text{観測方程式} : \quad \boldsymbol{y}_k &= \boldsymbol{H}_k \boldsymbol{x}_k + \boldsymbol{v}_k \label{eq:observation} \\ \text{状態方程式} : \quad \boldsymbol{x}_k &= \boldsymbol{F}_k \boldsymbol{x}_{k-1} + \boldsymbol{w}_k \label{eq:state} \end{align}

雑音 $\boldsymbol{w}_k, \boldsymbol{v}_k$ は平均 $\boldsymbol{0}$、かつ他の信号とは無相関で、分散共分散行列 $\boldsymbol{Q}_k = E[\boldsymbol{w}_k\boldsymbol{w}_k^T]$、$\boldsymbol{R}_k = E[\boldsymbol{v}_k\boldsymbol{v}_k^T]$ は既知とする。

推定誤差パワーの期待値を導く

何を最小化するのか

最小化対象は推定誤差のパワーの期待値 $E[\|\boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k}\|^2]$ である。これは推定誤差共分散行列 $\boldsymbol{P}_{k|k}$ のトレースに等しい：

$$E[\|\boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k}\|^2] = \text{tr}(\boldsymbol{P}_{k|k}), \quad \boldsymbol{P}_{k|k} = E[(\boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k})(\boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k})^T]$$

$\boldsymbol{P}_{k|k}$ の対角成分 $(P_{k|k})_{ii}$ は $i$ 番目の状態変数の推定誤差分散 $E[(x_{k,i} - \hat{x}_{k|k,i})^2]$ であるから、 $\text{tr}(\boldsymbol{P}_{k|k}) = \sum_i (P_{k|k})_{ii}$ はすべての状態変数の推定誤差分散の合計である。これを最小化することで、全体として最も精度の高い推定が得られる。

誤差信号の式

Kalman フィルタの構成 (概要ページ参照) は、予測した観測値と実際の観測値の差を使って状態推定を補正する形をとる。予測観測値は $\hat{\boldsymbol{y}}_k = \boldsymbol{H}_k \hat{\boldsymbol{x}}_{k|k-1}$ であるから、誤差信号 (イノベーション) $\boldsymbol{e}_k$ は

\begin{align} \boldsymbol{e}_k &= \boldsymbol{y}_k - \hat{\boldsymbol{y}}_k \\ &= \boldsymbol{y}_k - \boldsymbol{H}_k \hat{\boldsymbol{x}}_{k|k-1} \label{eq:ek} \end{align}

状態推定値 $\hat{\boldsymbol{x}}_{k|k}$ は、予測値 $\hat{\boldsymbol{x}}_{k|k-1}$ にこの誤差を線形に足し込む形 (線形推定器の仮定) を置く:

\begin{equation} \hat{\boldsymbol{x}}_{k|k} = \hat{\boldsymbol{x}}_{k|k-1} + \boldsymbol{K}_k \boldsymbol{e}_k \label{eq:xkk} \end{equation}

ここで行列 $\boldsymbol{K}_k$ は未知の重みであり、この後で「推定誤差パワーを最小にする $\boldsymbol{K}_k$」を求めるのが本導出の目標である。この式から、状態の推定誤差 $\boldsymbol{x}_k-\hat{\boldsymbol{x}}_{k|k}$ は

\begin{align} \boldsymbol{x}_k-\hat{\boldsymbol{x}}_{k|k} &= \boldsymbol{x}_k-(\hat{\boldsymbol{x}}_{k|k-1} + \boldsymbol{K}_k \boldsymbol{e}_k) \\ &\quad \text{式}\eqref{eq:ek}\text{より} \notag \\ &= \boldsymbol{x}_k-\left\{\hat{\boldsymbol{x}}_{k|k-1} + \boldsymbol{K}_k (\boldsymbol{y}_k - \boldsymbol{H}_k \hat{\boldsymbol{x}}_{k|k-1})\right\} \\ &\quad \text{式}\eqref{eq:observation}\text{により } \boldsymbol{y}_k \text{ を未知の状態 } \boldsymbol{x}_k \text{ を使った式に置き換えて} \notag \\ &= \boldsymbol{x}_k-\left\{\hat{\boldsymbol{x}}_{k|k-1} + \boldsymbol{K}_k (\boldsymbol{H}_k \boldsymbol{x}_k + \boldsymbol{v}_k - \boldsymbol{H}_k \hat{\boldsymbol{x}}_{k|k-1})\right\} \\ &= \boldsymbol{x}_k-\hat{\boldsymbol{x}}_{k|k-1} - \boldsymbol{K}_k (\boldsymbol{H}_k \boldsymbol{x}_k + \boldsymbol{v}_k - \boldsymbol{H}_k \hat{\boldsymbol{x}}_{k|k-1}) \\ &= (\boldsymbol{x}_k-\hat{\boldsymbol{x}}_{k|k-1}) - \boldsymbol{K}_k \boldsymbol{H}_k ( \boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k-1}) - \boldsymbol{K}_k \boldsymbol{v}_k \\ &= (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k)( \boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k-1}) - \boldsymbol{K}_k \boldsymbol{v}_k \label{eq:estimation-error} \end{align}

と書け、そのパワーの期待値は

\begin{equation} E[\|\boldsymbol{x}_k-\hat{\boldsymbol{x}}_{k|k}\|^2] = E\left[\left\|(\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k)( \boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k-1}) - \boldsymbol{K}_k \boldsymbol{v}_k\right\|^2\right] \label{eq:error-power-1} \end{equation}

無相関信号のノルムの性質: ベクトル $\boldsymbol{a}, \boldsymbol{b}$ (少なくとも一方は平均 $\boldsymbol{0}$) が無相関、すなわち $E[\boldsymbol{a}\boldsymbol{b}^T] = \boldsymbol{0}$ なら \begin{align} E[\|\boldsymbol{a}+\boldsymbol{b}\|^2] &= E[(\boldsymbol{a}+\boldsymbol{b})^T(\boldsymbol{a}+\boldsymbol{b})] \\ &= E[\boldsymbol{a}^T\boldsymbol{a}] + E[\boldsymbol{a}^T\boldsymbol{b}] + E[\boldsymbol{b}^T\boldsymbol{a}] + E[\boldsymbol{b}^T\boldsymbol{b}] \\ &= E[\|\boldsymbol{a}\|^2] + E[\|\boldsymbol{b}\|^2] \end{align} が成り立つ。クロス項については $E[\boldsymbol{a}^T\boldsymbol{b}] = \operatorname{tr}(E[\boldsymbol{b}\boldsymbol{a}^T]) = \operatorname{tr}(\boldsymbol{0}) = 0$ である ($\boldsymbol{a}^T\boldsymbol{b}$ はスカラーだからトレースに等しい)。

雑音 $\boldsymbol{v}_k$ は平均 0 で他の信号 $\boldsymbol{x}_k,\ \hat{\boldsymbol{x}}_{k|k-1}$ と無相関だから、式\eqref{eq:error-power-1}は次のように書ける。

\begin{equation} E[\|\boldsymbol{x}_k-\hat{\boldsymbol{x}}_{k|k}\|^2] = E\left[\left\|(\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k)( \boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k-1})\right\|^2\right] + E\left[\left\|\boldsymbol{K}_k \boldsymbol{v}_k\right\|^2\right] \label{eq:error-power-2} \end{equation}

トレースとノルムの関係： 列ベクトル $\boldsymbol{a}$ に対して $$\|\boldsymbol{a}\|^2 = a_0^2+a_1^2+\cdots+a_{N-1}^2 = \text{tr}[\boldsymbol{a}\boldsymbol{a}^T]$$ が成り立つ。これは $\boldsymbol{a}\boldsymbol{a}^T$ の対角成分が $a_i^2$ であることから分かる。

この関係を使うと、式\eqref{eq:error-power-2}は

\begin{align} E[\|\boldsymbol{x}_k-\hat{\boldsymbol{x}}_{k|k}\|^2] &= E\left[\text{tr}\left[(\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k)( \boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k-1}) ( \boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k-1})^T (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k)^T\right]\right] \notag \\ &\quad + E\left[\text{tr}\left[\boldsymbol{K}_k \boldsymbol{v}_k\boldsymbol{v}_k^T \boldsymbol{K}_k^T\right]\right] \end{align}

$\boldsymbol{K}_k$ や $\boldsymbol{H}_k$ は確定しているから期待値演算 $E[\cdot]$ の外に出せて、

\begin{align} E[\|\boldsymbol{x}_k-\hat{\boldsymbol{x}}_{k|k}\|^2] &= \text{tr}\left[(\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k) E\left[( \boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k-1}) ( \boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k-1})^T \right] (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k)^T\right] \notag \\ &\quad + \text{tr}\left[\boldsymbol{K}_k E\left[ \boldsymbol{v}_k\boldsymbol{v}_k^T \right] \boldsymbol{K}_k^T\right] \end{align}

ここで予測誤差の分散共分散行列を

\begin{equation} \boldsymbol{P}_{k|k-1} = E[(\boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k-1})(\boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k-1})^T] \label{eq:Pkk-1} \end{equation}

と書くことにし、$\boldsymbol{R}_k = E[\boldsymbol{v}_k\boldsymbol{v}_k^T]$ であったから、

\begin{align} E[\|\boldsymbol{x}_k-\hat{\boldsymbol{x}}_{k|k}\|^2] &= \text{tr}[(\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k) \boldsymbol{P}_{k|k-1} (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k)^T] + \text{tr}[\boldsymbol{K}_k \boldsymbol{R}_k \boldsymbol{K}_k^T] \end{align}

第1項を展開すると

\begin{align} &\text{tr}[(\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k) \boldsymbol{P}_{k|k-1} (\boldsymbol{I} - \boldsymbol{H}_k^T \boldsymbol{K}_k^T)] \\ &= \text{tr}[\boldsymbol{P}_{k|k-1} - \boldsymbol{K}_k \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} - \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T \boldsymbol{K}_k^T + \boldsymbol{K}_k \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T \boldsymbol{K}_k^T] \end{align}

ここで

\begin{equation} \boldsymbol{S}_k = \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T + \boldsymbol{R}_k \label{eq:Sk} \end{equation}

と書くことにすれば

\begin{equation} E[\|\boldsymbol{x}_k-\hat{\boldsymbol{x}}_{k|k}\|^2] = \text{tr}\left[ \boldsymbol{P}_{k|k-1} - \boldsymbol{K}_k \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} - \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T \boldsymbol{K}_k^T + \boldsymbol{K}_k \boldsymbol{S}_k \boldsymbol{K}_k^T \right] \label{eq:error-power-final} \end{equation}

となり、これを最小化すればよいことになる。

最適な Kalmanゲイン $\boldsymbol{K}_k$ の導出

Kalmanフィルタ内で自由に変えられるのは $\boldsymbol{K}_k$ のみである。推定誤差パワーの期待値を最小にするには $\boldsymbol{K}_k$ をどのように定めたらよいだろうか。

推定誤差 $\boldsymbol{x}_k-\hat{\boldsymbol{x}}_{k|k}$ のパワーの期待値が最小となる $\boldsymbol{K}_k$ では

\begin{equation} \frac{\partial}{\partial \boldsymbol{K}_k} E[\|\boldsymbol{x}_k-\hat{\boldsymbol{x}}_{k|k}\|^2] = 0 \label{eq:partial-zero} \end{equation}

が成り立つはずである。

トレースの行列微分の公式：

$\displaystyle\frac{\partial}{\partial \boldsymbol{X}} \text{tr}[\boldsymbol{XA}] = \boldsymbol{A}^T$	（証明）
$\displaystyle\frac{\partial}{\partial \boldsymbol{X}} \text{tr}[\boldsymbol{AX^T}] = \boldsymbol{A}$	（証明）
$\displaystyle\frac{\partial}{\partial \boldsymbol{X}} \text{tr}[\boldsymbol{XAX^T}] = \boldsymbol{X}(\boldsymbol{A}+\boldsymbol{A}^T)$	（証明）

これらの公式を式\eqref{eq:error-power-final}の各項に適用する。$\boldsymbol{P}_{k|k-1}$ 項は $\boldsymbol{K}_k$ に依存しないので微分は 0。残りについて:

$\text{tr}[\boldsymbol{K}_k (\boldsymbol{H}_k \boldsymbol{P}_{k|k-1})]$ は $\text{tr}[\boldsymbol{X}\boldsymbol{A}]$ の形 ($\boldsymbol{A}=\boldsymbol{H}_k \boldsymbol{P}_{k|k-1}$) で、微分は $\boldsymbol{A}^T = \boldsymbol{P}_{k|k-1}^T \boldsymbol{H}_k^T$
$\text{tr}[(\boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T) \boldsymbol{K}_k^T]$ は $\text{tr}[\boldsymbol{A}\boldsymbol{X}^T]$ の形で、微分は $\boldsymbol{A} = \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T$
$\text{tr}[\boldsymbol{K}_k \boldsymbol{S}_k \boldsymbol{K}_k^T]$ は $\text{tr}[\boldsymbol{X}\boldsymbol{A}\boldsymbol{X}^T]$ の形 ($\boldsymbol{A}=\boldsymbol{S}_k$) で、微分は $\boldsymbol{K}_k (\boldsymbol{S}_k + \boldsymbol{S}_k^T)$

これらを符号に注意して足し合わせて:

\begin{align} \frac{\partial}{\partial \boldsymbol{K}_k} E[\|\boldsymbol{x}_k-\hat{\boldsymbol{x}}_{k|k}\|^2] &= \frac{\partial}{\partial \boldsymbol{K}_k} \text{tr}\left[ \boldsymbol{P}_{k|k-1} - \boldsymbol{K}_k \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} - \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T \boldsymbol{K}_k^T + \boldsymbol{K}_k \boldsymbol{S}_k \boldsymbol{K}_k^T \right] \\ &= - (\boldsymbol{H}_k \boldsymbol{P}_{k|k-1})^T - \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T + \boldsymbol{K}_k (\boldsymbol{S}_k + \boldsymbol{S}_k^T) \\ &= - \boldsymbol{P}_{k|k-1}^T \boldsymbol{H}_k^T - \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T + \boldsymbol{K}_k (\boldsymbol{S}_k + \boldsymbol{S}_k^T) \label{eq:ddK} \end{align}

$\boldsymbol{P}_{k|k-1}, \boldsymbol{S}_k$ は対称行列（分散共分散行列は常に対称）であるから、 $\boldsymbol{P}_{k|k-1}^T = \boldsymbol{P}_{k|k-1}$、$\boldsymbol{S}_k^T = \boldsymbol{S}_k$ より

\begin{align} \frac{\partial}{\partial \boldsymbol{K}_k} E[\|\boldsymbol{x}_k-\hat{\boldsymbol{x}}_{k|k}\|^2] &= - 2\boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T + 2\boldsymbol{K}_k \boldsymbol{S}_k = 0 \end{align}

従って

\begin{equation} \boldsymbol{K}_k \boldsymbol{S}_k = \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T \label{eq:KS-PH} \end{equation}

両辺に右側から $\boldsymbol{S}_k^{-1}$ を掛けて

Kalmanゲイン

\begin{equation} \boldsymbol{K}_k = \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T \boldsymbol{S}_k^{-1} \label{eq:Kk} \end{equation}

としたとき、推定誤差 $\boldsymbol{x}_k-\hat{\boldsymbol{x}}_{k|k}$ のパワーの期待値が最小となる。

予測誤差の分散共分散行列 $\boldsymbol{P}_{k|k-1}$

式\eqref{eq:Kk}の計算に必要な $\boldsymbol{P}_{k|k-1} = E[(\boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k-1})(\boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k-1})^T]$ には、未知の状態 $\boldsymbol{x}_k$ が含まれており、このままでは Kalmanゲイン $\boldsymbol{K}_k$ を計算できない。そこでまず、1サンプル前の推定誤差の分散共分散行列 $\boldsymbol{P}_{k-1|k-1}$ から $\boldsymbol{P}_{k|k-1}$ を求める式を導く。

式\eqref{eq:state}から状態 $\boldsymbol{x}_k$ は $\boldsymbol{x}_k = \boldsymbol{F}_k \boldsymbol{x}_{k-1} + \boldsymbol{w}_k$ で推移する。また、予測値 $\hat{\boldsymbol{x}}_{k|k-1}$ は

\begin{equation} \hat{\boldsymbol{x}}_{k|k-1} = \boldsymbol{F}_k \hat{\boldsymbol{x}}_{k-1|k-1} \label{eq:xkk-1} \end{equation}

である。したがって予測値 $\hat{\boldsymbol{x}}_{k|k-1}$ の状態 $\boldsymbol{x}_k$ に対する誤差は

\begin{align} \boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k-1} &= \boldsymbol{F}_k \boldsymbol{x}_{k-1} + \boldsymbol{w}_k - \boldsymbol{F}_k \hat{\boldsymbol{x}}_{k-1|k-1} \\ &= \boldsymbol{F}_k (\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1|k-1}) + \boldsymbol{w}_k \end{align}

となり、式\eqref{eq:Pkk-1}は

\begin{align} \boldsymbol{P}_{k|k-1} &= E[(\boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k-1})(\boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k-1})^T] \\ &= E[\{\boldsymbol{F}_k (\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1|k-1}) + \boldsymbol{w}_k\}\{\boldsymbol{F}_k (\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1|k-1}) + \boldsymbol{w}_k\}^T] \end{align}

$\boldsymbol{w}_k$ は時刻 $k-1$ 以前の信号 $\boldsymbol{x}_{k-1}, \hat{\boldsymbol{x}}_{k-1|k-1}$ と無相関で平均 $\boldsymbol{0}$ であるから、積を展開したときの 2 つのクロス項 $E[\boldsymbol{F}_k(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1|k-1})\boldsymbol{w}_k^T]$ と $E[\boldsymbol{w}_k (\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1|k-1})^T \boldsymbol{F}_k^T]$ はいずれも $\boldsymbol{0}$ になる。したがって

\begin{align} \boldsymbol{P}_{k|k-1} &= E[\boldsymbol{F}_k (\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1|k-1})(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1|k-1})^T \boldsymbol{F}_k^T] + E[\boldsymbol{w}_k \boldsymbol{w}_k^T] \\ &= \boldsymbol{F}_k E[(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1|k-1})(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1|k-1})^T] \boldsymbol{F}_k^T + \boldsymbol{Q}_k \end{align}

ここで推定誤差の分散共分散行列を

\begin{equation} \boldsymbol{P}_{k|k} = E[(\boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k})(\boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k})^T] \label{eq:Pkk} \end{equation}

と書くことにすると、1サンプル前は $\boldsymbol{P}_{k-1|k-1} = E[(\boldsymbol{x}_{k-1} - \hat{\boldsymbol{x}}_{k-1|k-1})(\boldsymbol{x}_{k-1} - \hat{\boldsymbol{x}}_{k-1|k-1})^T]$ だから、

予測誤差の分散共分散行列

\begin{equation} \boldsymbol{P}_{k|k-1} = \boldsymbol{F}_k \boldsymbol{P}_{k-1|k-1} \boldsymbol{F}_k^T + \boldsymbol{Q}_k \label{eq:Pkk-1-result} \end{equation}

これにより、状態 $\boldsymbol{x}_k$ が分からなくても、$\boldsymbol{P}_{k-1|k-1}$ が計算してあれば、Kalmanゲイン $\boldsymbol{K}_k$ が計算可能になる。

推定誤差の分散共分散行列 $\boldsymbol{P}_{k|k}$

式\eqref{eq:Pkk-1-result}の $\boldsymbol{P}_{k-1|k-1}$ の計算法を述べる。

推定値 $\hat{\boldsymbol{x}}_{k|k}$ の誤差を表す式\eqref{eq:estimation-error} $$\boldsymbol{x}_k-\hat{\boldsymbol{x}}_{k|k} = (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k)( \boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k-1}) - \boldsymbol{K}_k \boldsymbol{v}_k$$ から推定誤差の分散共分散行列 $\boldsymbol{P}_{k|k}$ を求める。

\begin{align} \boldsymbol{P}_{k|k} &= E[(\boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k})(\boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k})^T] \\ &= E[\{(\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k)( \boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k-1}) - \boldsymbol{K}_k \boldsymbol{v}_k\}\{(\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k)( \boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k-1}) - \boldsymbol{K}_k \boldsymbol{v}_k\}^T] \end{align}

$\boldsymbol{v}_k$ は $\boldsymbol{x}_k, \hat{\boldsymbol{x}}_{k|k-1}$ と無相関で平均 $\boldsymbol{0}$ であるから、展開後のクロス項 $E[(\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k)(\boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k-1}) \boldsymbol{v}_k^T \boldsymbol{K}_k^T]$ と転置は $\boldsymbol{0}$ になる。残る 2 項をまとめると

\begin{align} \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{align}

展開して

\begin{align} \boldsymbol{P}_{k|k} &= \boldsymbol{P}_{k|k-1} - \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T \boldsymbol{K}_k^T - \boldsymbol{K}_k \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} + \boldsymbol{K}_k \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T \boldsymbol{K}_k^T + \boldsymbol{K}_k \boldsymbol{R}_k \boldsymbol{K}_k^T \\ &= \boldsymbol{P}_{k|k-1} - \boldsymbol{K}_k \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} - \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T \boldsymbol{K}_k^T + \boldsymbol{K}_k (\boldsymbol{H}_k \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T + \boldsymbol{R}_k)\boldsymbol{K}_k^T \end{align}

式\eqref{eq:Sk}より $\boldsymbol{S}_k = \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T + \boldsymbol{R}_k$ だから

\begin{align} \boldsymbol{P}_{k|k} &= \boldsymbol{P}_{k|k-1} - \boldsymbol{K}_k \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} + \boldsymbol{K}_k \boldsymbol{S}_k \boldsymbol{K}_k^T - \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T \boldsymbol{K}_k^T \\ &= (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k) \boldsymbol{P}_{k|k-1} + (\boldsymbol{K}_k \boldsymbol{S}_k - \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T) \boldsymbol{K}_k^T \end{align}

式\eqref{eq:KS-PH}より $\boldsymbol{K}_k \boldsymbol{S}_k = \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T$ だから第2項は 0 になり

推定誤差の分散共分散行列

\begin{equation} \boldsymbol{P}_{k|k} = (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k) \boldsymbol{P}_{k|k-1} \label{eq:Pkk-result} \end{equation}

この計算を前の時刻 $k-1$ でしておけば $\boldsymbol{P}_{k-1|k-1}$ が得られ、式\eqref{eq:Pkk-1-result}で $\boldsymbol{P}_{k|k-1}$ が計算可能になる。
つまりこの計算は、次の時刻で使うためにしておくのである。