概要

定常カルマンフィルタとリカッチ方程式について
結構，リカッチ方程式・評価関数について記述している記事が見当たらなかったため

参考図書：カルマンフィルタと適応信号処理　谷萩隆嗣著

先に結論

リカッチ方程式

P = APA^T +DWD^T - PC^T(V-CPC^T)^{-1}CP

オブザーバシステム

\hat{x}(t|Y(t))  = \hat{x}(t|Y(t-1)) + PC^TV^{-1} (y(t) - C \hat{x}(t|Y(t-1)))

カルマンゲイン$PC^TV^{-1} $

となります．

目的関数

次のような状態を考えます．

入力と出力を記憶している状況です．
システムは次の確率離散システムに従うとします．

\begin{align*}
\boldsymbol{x}(t) &= \boldsymbol{Ax}(t) + \boldsymbol{Bu}(t) + D w(t)\\
\boldsymbol{y}(t) &= \boldsymbol{Cx}(t)+v(t)
\end{align*}

離散システムなので，$n$を使おうかと思いましたが，時刻というのをわかりやすくするため$t$を使います．
$v,w$は

白色雑音
全ての信号と独立
平均$0$
共分散$E[v v^T] = V, E[w w^T] = W$

とします．

現在時刻は$t$とし，$y(0),...,y(t-1),y(t)$までコンピュータ上に保存されているとき**$x(t)$を推定すること**が今回の目標です．
$y(0),...,y(t-1) = Y(t-1)$と今後記述します．

推定すること＝(推定した値-真値の値)^2を小さくすること
です．よって，目標関数$J$は

J=E[ \{ x(t)-\hat{x}(t)\}^T \{x(t)-\hat{x}(t)\} | Y(t)] \\
\hat{x(t)}:推定値\\
E:期待値・平均値

となります．少し飛躍しています．
まず，なぜ期待値というのが出てきたのかというと$v,w$という確率ノイズのため$x(t)$が確定値にならないためです．よって，平均値を使うのが妥当だと考えられ，使われています．
LQRとかでは時間積分が入りましたが，時刻$t$のみでの状態推定が目標なので入らないです．

用語説明

$E(A|B)$というのは，現象$B$が起きた時の現象$A$の期待値を表します．

図中の$p(A|B)$は確率分布関数と呼ばれるものです．高校数学での確率は表・裏のような離散値を取っていましたが，今考えているのは$A=1.0$や$A=1.1$のように連続的な数値を取る状況を考えています．
ここで$A=1.00001$を取る確率は限りなく0です．なぜなら，$A=1.00002$のようにわずかなずれで$A=1.00001$から外れてしまうためです．よって，確率は「$1.0 \leq A \leq 1.2$を取る確率」のように範囲で考えることになります．つまり，

確率 = \int^{1.2}_{1.0} p(A|B) dA

のように積分で求めることになります．

正規分布について

ベクトル変数$x$(次数$n$)が正規分布に$Gause(\mu,\Sigma)$に従う
平均$\mu$,共分散行列$\Sigma$ の正規分布とする

数式では

p(x) = \frac{1}{\sqrt{(2 \pi )^n |\Sigma |} } \exp ( - \frac{1}{2}(x - \mu)^T \Sigma^{-1} (x - \mu) )

という確率分布になるということを意味しています．
参考: https://ja.wikipedia.org/wiki/%E5%A4%9A%E5%A4%89%E9%87%8F%E6%AD%A3%E8%A6%8F%E5%88%86%E5%B8%83

評価関数の変形

評価関数を変形します．

\begin{align}
J&=&E[(x(t)-\hat{x}(t))^T(x(t)-\hat{x}(t)) | Y(t)] \\
&=&  \hat{x}(t)^T \hat{x}(t)　-2\hat{x}(t)^T E[x(t)|Y(t)] +E[x(t)^Tx(t)|Y(t)]\\
&=& \{ \hat{x}(t) -E[x(t)|Y(t)] \} ^T \{ \hat{x}(t) -E[x(t)|Y(t)] \} +E[x(t)^Tx(t) | Y(t)] -E[x(t)|Y(t)] ^T E
[x(t)|Y(t)]\\
\end{align}

$E[x(t)^Tx(t) | Y(t)] -E[x(t)|Y(t)] ^T E[x(t)|Y(t)] $の項は$x(t)$のみで決まる．$x(t)$は$\hat{x}(t)$に由来しない．
よって，$J$を最小化するには

\hat{x}(t)  = E[x(t)|Y(t)]

とすればよいと考えられる!

推定量の書き方

推定値に$\hat{x}(t)$を使ってきたが，「$Y(t)$から推定した」という情報も導入し$\hat{x}(t|Y(t))$と書くことにする．

p(x(t)|Y(t))

以上より確率$p(x(t)|Y(t))$を求めることができれば，最適な推定値$\hat{x}(t|Y(t))$を計算できるということになる．
$p(x(t)|Y(t))$をベイズの定理により変形していく.
以降では，すべての変数は正規分布に従うと仮定する．

\begin{align}
p(x(t)|Y(t)) &=& \frac {p(x(t) \cap Y(t) )}{p(Y(t))} \\
&=& \frac {p(x(t) \cap Y(t-1) \cap y(t) )}{p(Y(t-1) \cap y(t) )} \\
&=& \frac {p(y(t)|x(t) ) p(x(t) | Y(t-1)) p(Y(t-1))}{p(y(t) |Y(t-1) ) p(Y(t-1))} \\
&=& \frac {p(y(t)|x(t) ) p(x(t) | Y(t-1))}{p(y(t) |Y(t-1) ) } \\
\end{align}

上の変形は，ベイズの定理

p(A \cap B) = p(B|A) p(A)

を使用した．さらに


\begin{align}
p(x(t) \cap Y(t-1) \cap y(t) ) &=& p(x(t) \cap y(t) |Y(t-1) p(Y(t-1))\\
 &=& p(y(t)|x(t) \cap Y(t-1)) p(x(t) | Y(t-1)) p(Y(t-1))\\
&=& p(y(t)|x(t) ) p(x(t) | Y(t-1)) p(Y(t-1))\\
\end{align}

を使用した．
2行目から3行目への変形

 p(y(t)|x(t) \cap Y(t-1)) = p(y(t)|x(t) )

は$y(t) = Cx(t) + v(t)$であり，$Y(t-1)$によらないためできた変形である．

まとめ

p(x(t)|Y(t))  = \frac {p(y(t)|x(t) ) p(x(t) | Y(t-1))}{p(y(t) |Y(t-1) ) } \\

以降では右辺の3つの確率分布を計算し，左辺の$p(x(t)|Y(t))$を計算していく．

p(y(t)|x(t))

 y(t) = Cx(t) + v(t)

であるため一番簡単．条件付確率のため$x(t)$は確定値とみる．よって$y(t)$は平均$Cx(t)$，共分散行列$V$の正規分布に従う

追記

E[\{y(t) - Cx(t) \} ^T \{y(t) - Cx(t) \}  ] = E[v(t)^T v(t)]\\
 = V

となる．

p(y(t) |Y(t-1))

\begin{align}
p(y(t) |Y(t-1) ) &=& p(  Cx(t) + v(t)  |Y(t-1) )\\
&=& p(  Cx(t)   |Y(t-1) ) + p(  v(t)  |Y(t-1) )\\
\end{align}

とできる．$v(t)$は$x(t)$と独立のためできたことである．平均値は

\begin{align}
E[y(t) |Y(t-1) ] &=& E[  Cx(t)   |Y(t-1) ]+ E[  v(t)  |Y(t-1) ]\\
&=& C \hat{x}(t|Y(t-1)) 
\end{align}

この変形は

\hat{x}(t|Y(t))  = E[x(t)|Y(t)]

と以前に定義したためできた変形である．

共分散は

\begin{align}
cov[y(t) |Y(t-1) ] &=& E[\{y(t) - C \hat{x}(t|Y(t-1)) \} \{y(t) - C \hat{x}(t|Y(t-1)) \} ^T ]\\
&=& E[\{Cx(t) +v(t)  - C \hat{x}(t|Y(t-1)) \} \{Cx(t) +v(t)- C \hat{x}(t|Y(t-1)) \} ^T ]\\
\end{align}

ここで誤差ベクトル$e$を定義する

e(t|Y(t-1))=x(t) -  \hat{x}(t|Y(t-1))

とする．共分散は

\begin{align}
cov[y(t) |Y(t-1) ] &=& E[\{Cx(t) +v(t)  - C \hat{x}(t|Y(t-1)) \} \{Cx(t) +v(t)- C \hat{x}(t|Y(t-1)) \} ^T ]\\
&=& E[\{Ce(t|Y(t-1)) +v(t)  \} \{Ce(t|Y(t-1)) +v(t)  \}^T ]\\
&=&C E[e(t|Y(t-1)) e(t|Y(t-1))^T]C^T +V
\end{align}

最後の変形は$v,e$が互いに独立のためできた変形である．

P(t|Y(t-1)) = E[e(t|Y(t-1)) e(t|Y(t-1))^T]

と新しく$P$を定義する．

まとめ

$p(y(t) |Y(t-1))$の

平均は$C \hat{x}(t|Y(t-1)) $
共分散は$CP(t|Y(t-1)C^T + V$

P(t|Y(t-1)) = E[e(t|Y(t-1)) e(t|Y(t-1))^T]

と定義した．

p(x(t) | Y(t-1))

最後である．

\begin{align*}
\boldsymbol{x}(t+1) &= \boldsymbol{Ax}(t) + \boldsymbol{Bu}(t) + D w(t)\\
\boldsymbol{y}(t) &= \boldsymbol{Cx}(t)+v(t)
\end{align*}

より，

p(x(t) | Y(t-1))= p(Ax(t-1) +Bu(t-1) + Dw(t-1) |Y(t-1))

$w(t-1)$は$x(t-1)$と独立，$u(t-1)$は時刻$t$では，$Y(t-1)$より決定済みの値である．よって，平均値は

\begin{align}
E[x(t) | Y(t-1)] &=& E[Ax(t-1)  |Y(t-1)] + Bu(t-1)\\
 &=& A \hat{x}(t-1|Y(t-1)) + Bu(t-1)\\
\end{align}

となる．また，$E[x(t) | Y(t-1)] = \hat{x}(t|Y(t-1))$なので

\begin{align}
\hat{x}(t|Y(t-1)) &=&  A \hat{x}(t-1|Y(t-1)) + Bu(t-1)\\
\end{align}

という，推定値の更新則にもなっている．

共分散は

\begin{align}
cov[x(t) | Y(t-1)] &=& E[ \{ x(t) - A\hat{x}(t-1|Y(t-1)) + Bu(t-1)  \} \{ x(t) - A\hat{x}(t-1|Y(t-1)) + Bu(t-1)  \} ^T]\\
&=& E[ \{ Ax(t-1) +Bu(t-1) +Dw(t-1)  - A\hat{x}(t-1|Y(t-1))  - Bu(t-1)  \}\\ \{ Ax(t-1) +Bu(t-1) +Dw(t-1)  - A\hat{x}(t-1|Y(t-1))  - Bu(t-1)  \}^T]\\
&=&E[ \{ Ae(t-1|Y(t-1))  +Dw(t-1)    \} \{ Ae(t-1|Y(t-1))  +Dw(t-1)    \}^T]\\
&=&AP(t-1|Y(t-1)) A^T +DWD^T
\end{align}

となった．最後の行への変形は

P(t|Y(t-1)) = E[e(t|Y(t-1)) e(t|Y(t-1))^T]

と，$w(t-1)$が$e(t-1|Y(t-1))$と独立なのを用いた．

この式はとても重要な式である．

\begin{align}
cov[x(t) | Y(t-1)] &=& E[ \{ x(t) -\hat{x}(t|Y(t-1))   \} \{ x(t) - \hat{x}(t|Y(t-1))   \} ^T]\\
&=& P(t|Y(t-1))
\end{align}

よって

P(t|Y(t-1)) = AP(t-1|Y(t-1)) A^T +DWD^T

となり，$P$行列の更新式になっている．

まとめ

平均$\hat{x}(t|Y(t-1))$
共分散$P(t|Y(t-1))$

に従う．推定値は

\begin{align}
\hat{x}(t|Y(t-1)) &=&  A \hat{x}(t-1|Y(t-1)) + Bu(t-1)\\
\end{align}

P(t|Y(t-1)) = AP(t-1|Y(t-1)) A^T +DWD^T

に従って更新する．
以上で必要な３つの確率分布が求まった．

かけ合わせ

p(x(t)|Y(t))  = \frac {p(y(t)|x(t) ) p(x(t) | Y(t-1))}{p(y(t) |Y(t-1) ) } \\

であった．

$p(y(t)|x(t) )$は

平均$Cx(t)$
共分散行列$V$

$p(x(t) | Y(t-1))$は

平均$\hat{x}(t|Y(t-1))$
共分散$P(t|Y(t-1))$

$p(y(t) |Y(t-1)$は

平均は$C \hat{x}(t|Y(t-1)) $
共分散は$CP(t|Y(t-1))C^T + V$

e(t|Y(t-1))=x(t) -  \hat{x}(t|Y(t-1))\\
P(t|Y(t-1)) = E[e(t|Y(t-1)) e(t|Y(t-1))^T]

という，2つの新しい変数を導入した．

推定値は

\begin{align}
\hat{x}(t|Y(t-1)) &=&  A \hat{x}(t-1|Y(t-1)) + Bu(t-1)\\
\end{align}

P(t|Y(t-1)) = AP(t-1|Y(t-1)) A^T +DWD^T

に従って更新する．

3つの確率分布をかけ合わせていこう．

ベクトル変数$x$(次数$n$)が正規分布に$Gause(\mu,\Sigma)$に従う
平均$\mu$,共分散行列$\Sigma$ の正規分布とする

p(x) = \frac{1}{\sqrt{(2 \pi )^n |\Sigma |} } \exp ( - \frac{1}{2}(x - \mu)^T \Sigma^{-1} (x - \mu) )

という形に書けるので，それをもとに実際に掛けていく．
読みやすさのために，次のように略記する

P=P(t|Y(t-1))\\
x=x(t)\\
\hat{x} = \hat{x}(t|Y(t-1))\\
y= y(t)\\
Y= Y(t-1)

\begin{align}
p(x(t)|Y(t))  &=& \frac {p(y(t)|x(t) ) p(x(t) | Y(t-1))}{p(y(t) |Y(t-1) ) } \\

&=& \frac{1}{\sqrt{(2 \pi )^n |V | |P| |CPC^T + V| ^{-1}}  } 
 \exp ( - \frac{1}{2} \{
(y - Cx) ^T V^{-1}(y - Cx)
+(x - \hat{x})^T P^{-1}(x - \hat{x})
-(y - C \hat{x})^T (CPC^T + V)^{-1} (y - C \hat{x} )^T
\})
\end{align}

ものすごい長い式だが，平均値と，誤差の共分散を求めよう！
指数部のみ抜き出す

\begin{align}
 \exp ( - \frac{1}{2} \{
(y - Cx) ^T V^{-1}(y - Cx)
+(x - \hat{x})^T P^{-1}(x - \hat{x})
-(y - C \hat{x})^T (CPC^T + V)^{-1} (y - C \hat{x} )^T
\})\\
 =\exp ( - \frac{1}{2} \{
(y - C \hat{x} - C( x - \hat{x})) ^T V^{-1}(y - C \hat{x} - C( x - \hat{x}))
+(x - \hat{x})^T P^{-1}(x - \hat{x})
-(y - C \hat{x})^T (CPC^T + V)^{-1} (y - C \hat{x} )^T
\})\\

=\exp ( - \frac{1}{2} \{

+(x - \hat{x})^T (P^{-1} +C^TV^{-1}C)(x - \hat{x})
+(y - C \hat{x})^T (-(CPC^T + V)^{-1} +V^{-1})(y - C \hat{x} )^T
-(y - C \hat{x})^T V^{-1} C(x - \hat{x})
-(x - \hat{x})^T C^TV^{-1}(y - C \hat{x})

\})

\end{align}

となる．

ここで，新しい変数$Q$を定義する

Q^{-1} =  P^{-1}  +C^T V^{-1} C

逆行列の補助定理より
https://mathtrain.jp/woodbury

(CP C^T+V)^{-1} = V^{-1}-V^{-1}C(P^{-1}+C^TV^{-1}C)^{-1} C^T V^{-1}\\
=V^{-1}-V^{-1}C Q C^T V^{-1}

となる．
指数部をまとめる作業に戻る

\exp ( - \frac{1}{2} \{

+(x - \hat{x})^T (P^{-1} +C^TV^{-1}C)(x - \hat{x})
+(y - C \hat{x})^T (-(CPC^T + V)^{-1} +V^{-1})(y - C \hat{x} )^T
-(y - C \hat{x})^T V^{-1} C(x - \hat{x})
-(x - \hat{x})^T C^TV^{-1}(y - C \hat{x})

\})\\
=
\exp ( - \frac{1}{2} \{

+(x - \hat{x})^T (Q^{-1})(x - \hat{x})
+(y - C \hat{x})^T (-V^{-1}+V^{-1}C Q C^T V^{-1} +V^{-1})(y - C \hat{x} )^T
-(y - C \hat{x})^T V^{-1} C(x - \hat{x})
-(x - \hat{x})^T C^TV^{-1}(y - C \hat{x})

\})\\

=
\exp ( - \frac{1}{2} \{

+(x - \hat{x})^T (Q^{-1})(x - \hat{x})
+(y - C \hat{x})^T (V^{-1}C Q C^T V^{-1})(y - C \hat{x} )^T
-(y - C \hat{x})^T V^{-1} C(x - \hat{x})
-(x - \hat{x})^T C^TV^{-1}(y - C \hat{x})

\})\\

=
\exp ( - \frac{1}{2} \{

(x - \hat{x} -QC^TV^{-1} (y - C \hat{x}) )^T (Q^{-1})(x - \hat{x} -QC^TV^{-1} (y - C \hat{x}) )


\})\\

よって，$p(x(t)|Y(t))$の

平均は$\hat{x} + QC^TV^{-1} (y - C \hat{x}) $
共分散は$Q = (P^{-1} +C^T V^{-1} C)^{-1}$

となる！

\hat{x}(t|Y(t))  = E[x(t)|Y(t)]

とするのが最適だったので

\hat{x}(t|Y(t))  =\hat{x}  + QC^TV^{-1} (y - C \hat{x})\\
= \hat{x}(t|Y(t-1)) + QC^TV^{-1} (y(t) - C \hat{x}(t|Y(t-1)))

とすればよいことになる！
また，共分散は

e(t|Y(t-1))=x(t) -  \hat{x}(t|Y(t-1))\\
P(t|Y(t-1)) = E[e(t|Y(t-1)) e(t|Y(t-1))^T]

の定義より$P(t|Y(t))$と一致する．

よって

Q = P(t|Y(t))

となる．

結論

\hat{x}(t|Y(t))  = \hat{x}(t|Y(t-1)) + P(t|Y(t))C^TV^{-1} (y(t) - C \hat{x}(t|Y(t-1)))\tag{1}

\begin{align}
\hat{x}(t|Y(t-1)) &=&  A \hat{x}(t-1|Y(t-1)) + Bu(t-1)\\\tag{2}
\end{align}

P(t|Y(t))^{-1} =  P(t|Y(t-1))^{-1}  +C^T V^{-1} C\tag{3}

P(t|Y(t-1)) = AP(t-1|Y(t-1)) A^T +DWD^T\tag{4}

(1):計測による推定値の更新
(2):未来時刻の推定
(3):共分散の観測による変化
(4):共分散の時間進展による変化

無限時間カルマンゲインを計算しようとすると
(3),(4)式はリカッチ方程式になります．
カルマンゲインは$P(t|Y(t))C^TV^{-1}$となります．

リカッチ方程式の導出

P(t|Y(t))^{-1} =  P(t|Y(t-1))^{-1}  +C^T V^{-1} C\tag{3}

P(t|Y(t-1)) = AP(t-1|Y(t-1)) A^T +DWD^T\tag{4}

をまとめることで
$P(t-1|Y(t-1))$ -> $P(t|Y(t))$への変化の式を作ります．

P(t|Y(t))^{-1} =   (AP(t-1|Y(t-1)) A^T +DWD^T )^{-1}  +C^T V^{-1} C \\

繰り返し計算の末$P(t|Y(t)) = P(t-1|Y(t-1)) = P$に収束したとすると

P^{-1} =   (APA^T +DWD^T )^{-1}  +C^T V^{-1} C \\

これが定常カルマンフィルタにおける，リカッチ方程式である！！
この式をさらに変形するとよく見る形に変形できる．逆行列補題を使用するhttps://mathtrain.jp/woodbury

APA^T +DWD^T  = (P^{-1} -C^TV^{-1}C )^{-1}\\
=P+PC^T(V-CPC^T)^{-1}CP\\

この式はよく見る式だ！

P = APA^T +DWD^T - PC^T(V-CPC^T)^{-1}CP

http://stlab.ssi.ist.hokudai.ac.jp/yuhyama/lecture/digital/digi-part2-4up.pdf
のp.9に同じ式がある

終わりに

かなり長い式変形があったり，あまりわかりやすくないと思うが，
リカッチ方程式をちゃんと導出したWEBページはなかなかないので，ほんとの一部のマニアには意義があると考えたい．
何か間違いがあれば，修正リクエストを送ってほしい．