カルマンフィルタ

誤差が正規分布であると考えられるときに現在の状態と誤差分布を求めることができる． ここでは構成法にのみ注目する． 詳しい解説についてはWikipediaか関連書籍を読むとよい．

カルマンフィルター - Wikipedia

利用するために必要な定式化

とりあえず決めなければならないことは， 状態ベクトル $x_t$ と計測ベクトル $z_t$ の構成である．

それができたら，状態方程式を構成する．

$$x_t = A_t x_{t-1} + B_t u_t + e_t$$

$u_t$は入力ベクトル，$e_t$は誤差ベクトルである． だが，ここで注目すべきは$e_t$は誤差項なので計算に直接は関わらないこと． 決めなければいけないのは$e_t$の共分散行列$R_t$である．

i.e. $R_t$は$x_t$の誤差共分散行列．

次に計測方程式を構成する． $$z_t = C_t x_t + d_t$$ 現在の状態から計測値を求めるということに注意．単なる推定の逆になっている． ここでも$d_t$は誤差項なので決めなくてよい. $d_t$の共分散行列$Q_t$を決める．

i.e. $Q_t$は$z_t$の誤差共分散行列．

まとめると，カルマンフィルタでするべきことは，

• $x_t$と$z_t$を決める．ついでに$u_t$も決める．
• $x_t$と$x_{t-1}$の関係から$A_t$を決める．
• $u_t$と$x_t$の関係から$B_t$を決める．
• $x_t$と$z_t$の関係から$C_t$を決める．
• $x_t$の構成要素から$R_t$を決める．
• $z_t$の構成要素から$Q_t$を決める．

肝は$R_t$と$Q_t$をどう決めるかである． 適当な値を入れても計算はできるが簡単に発散する． むやみに小さな値を入れても発散する． うまいこと調整するといい．

アルゴリズム

一時刻前の状態を${\mu}_{t-1}$ ,共分散を${\Sigma}_{t-1}$とすると，

• $\bar{{\mu}_t} = A_t {\mu}_{t-1} + B_t u_t$
• $\bar{{\Sigma}_{t-1}} = A_t {\Sigma}_{t-1} A_t^T + R_t$
• $K_t = \bar{{\Sigma}_{t-1}} C_t^T ( C_t \bar{{\Sigma}_{t-1}} C_t^T +Q_t )^{-1}$
• ${\mu}_t = \bar{{\mu}_t} + K_t ( z_t - C_t \bar{{\mu}_t} )$
• ${\Sigma}_t = ( I - K_t C_t ) \bar{{\Sigma}_{t-1}}$

拡張カルマンフィルタ

カルマンフィルタはすべてが線形でないと構成できない． 拡張カルマンフィルタは線形化を施すことで非線形に拡張したもの．

利用するために必要な定式化

まずは状態方程式と計測方程式を立てる．カルマンが非線形なだけだから問題ないはず． すなわち状態方程式と計測方程式はそれぞれ次のようになる．

$$x_t = g(x_{t-1},u_t) + e_t$$ $$z_t = h(x_t) + d_t$$

ここで問題なのは状態関数$g(x_{t-1},u_t)$と計測関数$h(x_t)$のヤコビアン$G_t,H_t$を求める必要があること． 場合によっては超難しいが高校数学を駆使すればできなくもない． あとはMathematicaを使おう．ただし最初から使うと泥沼化しかねないので注意．

拡張カルマンフィルタですべきことは,

• 基本的にはカルマンのときと一緒だが，
• $A_t$の代わりに$g(x_{t-1},u_t)$を決める．
• $C_t$の代わりに$h(x_t)$を決める．
• $g(x_{t-1},u_t)$の$x$に関するヤコビアン$G_t$を求める．
• $h(x_t)$の$x$に関するヤコビアン$H_t$を求める．

アルゴリズム

カルマンフィルタの一部を$g,h,G,H$に直しただけ．

• $\bar{{\mu}_t} = g( {\mu}_{t-1} . u_t )$
• $\bar{{\Sigma}_{t-1}} = G_t {\Sigma}_{t-1} G_t^T + R_t$
• $K_t = \bar{{\Sigma}_{t-1}} H_t^T ( H_t \bar{{\Sigma}_{t-1}} H_t^T +Q_t )^{-1}$
• ${\mu}_t = \bar{{\mu}_t} + K_t ( z_t - h( \bar{{\mu}_t} ))$
• ${\Sigma}_t = ( I - K_t H_t ) \bar{{\Sigma}_{t-1}}$

補足

• カルマンフィルタ，拡張カルマンフィルタは長いのでKF,EKFと略されることが多い．
• 誤差が正規分布にのっとらない場合は素直にパーティクルフィルタを使おう．

参考文献

• Thrun et. al.: "確率ロボティクス" , 毎日コミュニケーションズ , 2007.