LoginSignup
3
6

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 3 years have passed since last update.

@GANTZ(GANTZ G)

離散システムの定常カルマンフィルタとリカッチ方程式による解法

Last updated at Posted at 2021-02-07

概要

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

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

先に結論

リカッチ方程式

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} $

となります．

目的関数

次のような状態を考えます．
１.png
入力と出力を記憶している状況です．
システムは次の確率離散システムに従うとします．

\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$の期待値を表します．
2.png
図中の$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ページはなかなかないので，ほんとの一部のマニアには意義があると考えたい．
何か間違いがあれば，修正リクエストを送ってほしい．

3
6
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
3
6

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?