Probabilistic Robotics - Chap.3 Gaussian Filters

3.2 The Kalman Filter
- 3.2.2 The Kalman Filter Algorithm
3.3 The Extended Kalman Filter
- 3.3.1 Why Liearlization?
- 3.3.2 Linealization Via Taylor Expansion
3.4 The Unscented Kalman Filter(UKF)
- 3.4.2 The UKF Alogorithm
- 3.5 The Information Filter（IF)
参考

3.2 The Kalman Filter

カルマンフィルタは線形ガウスシステムでフィルタリング（つまり余計なものを省くプロセス[1]）と予測を行うために開発された。カルマンフィルタは、連続状態空間でのbeliefを実装する。離散状態空間やハイブリッドな空間には対応しない。

時刻 ${t}$ でのbelief ${\ bel(x_t)}$ は、ベイズフィルタがマルコフ過程に加えて下記３点を満たす時、平均 ${\mu_t}$ と共分散 ${\sigma_t}$ のガウス分布で表せる。

【１点目】
状態遷移確率 ${p(x_t | u_t, x_{t-1})}$ が線形関数で表わせ、状態がガウスノイズを加味した下記の式で表せる。
${x_t = A_t x_{t-1} + B_t u_t + \epsilon_t \tag{1}}$
ここで、 ${x_t、x_{t-1}}$ は状態ベクトル、 ${u_t}$ は制御ベクトルを、それぞれ表す。また ${A_t}$ は ${n \times n}$ の正方行列で、 ${n}$ は状態ベクトルの次元を示す。そして ${B_t}$ は ${n \times m}$ の行列で、 ${m}$ は制御ベクトルの次元を示す。このように行列 ${A_t、B_t}$ を導入することで、カルマンフィルタは入力である状態と制御の線形関数として状態遷移を表現する。
なお ${\epsilon_t}$ は、状態遷移の不確かさを表現するためのガウスノイズで、平均は ${0}$ 、共分散は ${R_t}$ 、次元は状態ベクトルと同じ ${n}$ とする。

${(1)}$ 式を多変量ガウス分布の式に当てはめると、平均 ${A_t x_{t-1} + B_t u_t}$ で共分散 ${R_t}$ である
${ p(x_t | u_t, x_{t-1}) = det(2 \pi R_t)^{- \frac{1}{2}} exp\{- \frac{1}{2} (x_t - A_t x_{t-1} - B_t u_t)^T R^{-1}_t (x_t - A_t x_{t-1} - B_t u_t)\} \tag{2} }$
として表せる。

【２点目】
計測確率 ${p(z_t | x_t)}$ も、状態遷移確率と同様で入力に対して線形とする。また計測データは、ガウスノイズを加味した下記の式で表せる。
${z_t = C_t x_t + \delta_t \tag{3}}$
ここで ${C_t}$ は ${k \times n}$ の行列で、 ${k}$ は計測ベクトル ${z_t}$ の次元とする。 ${\delta_t}$ は、平均 ${0}$ で共分散 ${Q_t}$ の計測ノイズである。計測確率は、 ${(2)}$ と同様に多変量ガウス分布の形で、
${ p(z_t | x_t) = det(2 \pi Q_t)^{- \frac{1}{2}} exp\{- \frac{1}{2} (z_t - C_t x_t)^T Q^{-1}_t (z_t - C_t x_t)\} \tag{4} }$
として表せる。

【３点目】
最後に、初期belief ${bel(x_0)}$ は、平均 ${\mu_0}$ で共分散 ${\Sigma_0}$ のガウス分布
${ bel(x_0) = p(x_0) = det(2 \pi \Sigma_0)^{- \frac{1}{2}} exp\{- \frac{1}{2} (x_0 - \mu_0)^T \Sigma^{-1}_0 (x_0 - \mu_0)\} \tag{5} }$
として表せる。

これら３つの仮定は、 ${bel(x_t)}$ がガウス分布で表せることを確認するのに十分である。

3.2.2 The Kalman Filter Algorithm

カルマンフィルタの入力と出力は
入力： ${平均\mu_{t-1}、共分散\Sigma_{t-1}のbel(x_{t-1})}$ と制御ベクトル ${u_t}$ 、観測ベクトル ${z_t}$
出力： ${平均\mu_t、共分散\Sigma_tのbel(x_t)}$
である。アルゴリズムは下記の数式で表せる。

${ Algorithm \ Kalman\_Filter(\mu_{t-1}, \Sigma_{t-1}, u_t, z_t):\\ \ \ \overline{\mu}_t = A_t \mu_{t-1} + B_t u_t\\ \ \ \overline{\Sigma}_t = A_t \Sigma_{t-1} A^T_t + R_t\\ \\ \ \ K_t = \overline{\Sigma}_t C^T_t (C_t \overline{\Sigma}_t C^T_t + Q_t)^{-1} \\ \ \ \mu_t = \overline{\mu}_t + K_t (z_t - C_t \overline{\mu}_t)\\ \ \ \Sigma_t = (I - K_t C_t) \overline{\Sigma}_t\\ \ \ return\ \mu_t, \Sigma_t }$
Table3.1 対象が線形な場合のカルマンフィルタアルゴリズム

なお ${K_t}$ はカルマンゲインと呼ばれ、状態遷移時に観測結果をどの程度考慮するかを調節する値である。

Table3.1の２，３行目は制御を加えた結果の予測過程を、４～６行目は観測結果を元に状態を更新する過程を、それぞれ示している。
それぞれの行に対する私の解釈は、

２行目：制御を加えたことによる平均の変化を予測
３行目：共分散 ${\Sigma_t}$ は分散の２乗であるから、 ${A_t}$ に対する二次形式になっている
５行目：予測した平均に対し、予測からどれだけずれたか、 ${z_t - C_t \overline{\mu_t}}$ を考慮して平均を更新
６行目：共分散を求める式なので、 ${I - K_t C_t}$ の部分は必ず正になる。よって ${0 \ \leq \ (I - K_t C_t)_i \ \leq \ 1}$ とわかる。このことから、共分散を抑えにかかっていると言える。

下記に私がPythonで書いたカルマンフィルタのコードを載せました。参考になれば。

3.3 The Extended Kalman Filter

3.3.1 Why Liearlization?

システムが線形であることは、現実的にはかなり稀である。そのため、本節では非線形なシステムにカルマンフィルタを適用する方法を検討する。今状態 ${x_t}$ と観測結果 ${z_t}$ がそれぞれ、
${ x_t = g(u_t, x_{t-1}) + \epsilon_t \\ z_t = h(x_t) + \delta_t }$
で表せるものとする。ここで ${g、h}$ は非線形関数とする。
上記２式は、線形カルマンフィルタで使った行列 ${A_t、B_t}$ を ${g}$ で、行列 ${C_t}$ を ${h}$ にそれぞれ置き換えた式と言える。つまり拡張カルマンフィルタは、基本的には線形カルマンフィルタと同じと言える。ただ一方で、非線形関数を用いることからbeliefはガウス分布の形を成さなくなる。

3.3.2 Linealization Via Taylor Expansion

EKFの重要なアイデアは、線形化である。 ${g、h}$ を線形化すると、EKFはLKFと同様にして振る舞う。
線形化には種々の方法があるが、EKFでは１次のテイラー展開[2]を用いる。よって ${g、h}$ はそれぞれ、

${ g(u_t, x_{t-1}) \approx g(u_t, \mu_{t-1}) + g^{'}(u_t, \mu_{t-1})(x_{t-1} - \mu_{t-1})\\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ = g(u_t, \mu_{t-1}) + G_t (x_{t-1} - \mu_{t-1})\\ \\ h(x_t) \approx h(\overline{\mu_t}) + h^{'}(\overline{\mu_t})(x_t - \overline{\mu_t})\\ \ \ \ \ \ \ \ \ \ = h(\overline{\mu_t}) + H_t (x_t - \overline{\mu_t}) }$

と近似できる。この結果を使い、状態遷移確率 ${p(x_t | u_t, x_{t-1})}$ と観測確率 ${p(z_t | x_t)}$ はそれぞれ、

${ p(x_t | u_t, x_{t-1}) \approx det(2 \pi R_t)^{- \frac{1}{2}} exp \{ - \frac{1}{2} [x_t - g(u_t, \mu_{t-1}) - G_t (x_{t-1} - \mu_{t-1} ]^T R^{-1}_t [x_t - g(u_t, \mu_{t-1}) - G_t (x_{t-1} - \mu_{t-1} ] \} \\ \\ p(z_t | x_t) = det(2 \pi R_t)^{- \frac{1}{2}} exp \{- \frac{1}{2} [ z_t - h(\overline{\mu}_t - H_t (x_t - \overline{\mu}_t) ]^T Q^{-1}_t [ z_t - h(\overline{\mu}_t - H_t (x_t - \overline{\mu}_t) ] \} }$

と表すことができる。

${ Algorithm \ Extended_Kalman\_Filter(\mu_{t-1}, \Sigma_{t-1}, u_t, z_t):\\ \ \ \overline{\mu}_t = g(u_t, \mu_{t-1})\\ \ \ \overline{\Sigma}_t = G_t \Sigma_{t-1} G^T_t + R_t\\ \\ \ \ K_t = \overline{\Sigma}_t H^T_t (H_t \overline{\Sigma}_t H^T_t + Q_t)^{-1} \\ \ \ \mu_t = \overline{\mu}_t + K_t (z_t - h(\overline{\mu}_t))\\ \ \ \Sigma_t = (I - K_t H_t) \overline{\Sigma}_t\\ \ \ return\ \mu_t, \Sigma_t }$

3.4 The Unscented Kalman Filter(UKF)

UKFも非線形なシステムに対応するカルマンフィルタの１手法。線形回帰を使うことで、統計的に線形化する。回帰に使うサンプル点として、Sigma Point[3]を設定する。Sigma Pointは、平均を中心とし、かつ共分散行列の主軸に対して線対称になるように配置する。平均 ${\mu}$ ・共分散 ${\Sigma}$ な ${n}$ 次元のガウス分布については、 ${2n+1}$ 個のSigma Pointを配置する。配置時のルールは下記の通り。

${ \begin{align} \chi^{[0]} &= \mu\\ \chi^{[i]} &= \mu + (\sqrt{(n + \lambda \Sigma})_i \ \ \ for\ \ i = 1,...,n \\ \chi^{[i]} &= \mu - (\sqrt{(n + \lambda \Sigma})_{i-n} \ \ \ for\ \ i = n+1,...,2n \\ \end{align} }$

ここで、 ${\lambda = \alpha^2(n + \kappa) - n}$ で、 ${\alpha}$ と ${\kappa}$ は ${\mu}$ からどの程度離れるかを決めるスケールパラメータである。

また、各 ${\chi^{[i]}}$ は２種類の重み ${w^{[i]}_m}$ と ${w^{[i]}_c}$ を持つ。 ${w^{[i]}_m}$ は平均を、 ${w^{[i]}_c}$ はガウス分布の共分散を、それぞれ計算する際に用いる。なお ${w^{[i]}_m}$ と ${w^{[i]}_c}$ は下記の式で求める。

${ \begin{align} w^{[0]}_m &= \frac{\lambda}{n + \lambda} \\ w^{[0]}_c &= \frac{\lambda}{n + \lambda} + (1 - \alpha^2 + \beta) \\ w^{[i]}_m &= w^{[i]}_c = \frac{1}{2(n + \lambda)} \ \ \ for\ \ i = 1,...,2n. \end{align} }$

ここで ${\beta}$ は、分布に関する追加情報をエンコードするためのパラメータであり、分布がガウス分布であれば ${\beta = 2}$ が最良となる。
　Sigma Pointは、関数 ${g}$ を使って写像される。写像後の平均 ${\mu^{'}}$ と共分散 ${\Sigma^{'}}$ はそれぞれ、
${ Y^{[i]} = g(\chi^{[i]}) \\ \displaystyle \mu^{'} = \sum^{2n}_{i=0} w^{[i]}_m Y^{[i]}\\ \Sigma^{'} = w^{[i]}_c (Y^{[i]} - \mu^{'} ) (Y^{[i]} - \mu^{'} )^T }$
として表せる。

3.4.2 The UKF Alogorithm

UKFとEKFの入出力は同じだが、計算負荷はUFKの方が少し高い。また線形システムでは、UFKとLKFは同じ結果を示す。UKFの利点は、ヤコビアンの計算が不要な点である。ヤコビアンの計算には微分が必要であり、時には計算が難しい。そのためヤコビアンの計算を回避できることは計算上好ましい。なおUKFはパーティクルフィルタに似ているが、パーティクルフィルタはサンプル点をランダムに取るのに対し、UKFではサンプル点の取り方が固定という違いがある。

${ Algorithm\ \ Unscented\_Kalman\_Filter(\mu_{t-1}, \Sigma_{t-1}, u_t, z_t):\\ \ \ \ \chi_{t-1} = (\mu_{t-1} \ \ \ \mu_{t-1} + \gamma \sqrt{\Sigma_{t-1}} \ \ \ \mu_{t-1} - \gamma \sqrt{\Sigma_{t-1}})\\ \ \ \ \overline{\chi}^{*}_t = g(u_t, \chi_{t-1})\\ \ \ \ \overline{\mu}_t = \sum^{2n}_{i=0} w^{[i]}_m \overline{\chi}^{*[i]}_t \\ \ \ \ \overline{\Sigma}_t = \sum^{2n}_{i=0} w^{[i]}_c (\overline{\chi}^{*[i]}_t - \overline{\mu}_t) (\overline{\chi}^{*[i]}_t - \overline{\mu}_t)^T + R_t \\ \ \ \ \overline{\chi}_t = (\overline{\mu}_t \ \ \ \overline{\mu}_t + \gamma \sqrt{\overline{\Sigma}_t} \ \ \ \overline{\mu}_t - \gamma \sqrt{\overline{\Sigma}_t})\\ \ \ \ \overline{Z}_t = h( \overline{\chi}_t)\\ \ \ \ \hat{z}_t = \sum^{2n}_{i=0} w^{[i]}_m \overline{Z}^{[i]}_t \\ \ \ \ S_t = \sum^{2n}_{i=0} w^{[i]}_c (\overline{Z}^{[i]}_t - \hat{z}_t) (\overline{Z}^{[i]}_t - \hat{z}_t)^T + Q_t \\ \ \ \ \sum^{x,z}_t = \sum^{2n}_{i=0} w^{[i]}_c (\overline{\chi}^{[i]}_t - \overline{\mu}_t) (\overline{Z}^{[i]}_t - \overline{\mu}_t)^T \\ \ \ \ K_t = \sum^{x,z}_t S^{-1}_t \\ \ \ \ \mu_t = \overline{\mu}_t + K_t ( z_t - \hat{z}_t )\\ \ \ \ \Sigma_t = \overline{\Sigma}_t - K_t S_t K_t^T \\ \ \ \ return \ \ \mu_t, \Sigma_t }$

3.5 The Information Filter（IF)

　LKFと同様に線形なシステムを仮定している。LKFとの主な違いは、ガウス分布の表現方法である。LKFとその派生は、ガウス分布をモーメント（平均と共分散）を使って表している。それに対しIFは、カノニカルな表現方法を使用する（＊注意：僕、カノニカルとはどういうことかよくわかっていません！調べている途中です。。）。

　IFとLKFは、状態遷移確率と観測確率それぞれを、LKFと同じ式で求める。またLKFと同様に、予測と更新の２ステップで構成される。IFの入力は、カノニカルなパラメータ ${\xi_{t-1}と\Omega_{t-1}、u_t、z_t}$ である。そして出力は ${\xi_tと\Omega_t}$ である。なお ${\xiと\Omega}$ の定義は下記の通り。
${ \begin{align} \Omega &= \Sigma^{-1} \\ \xi &= \Sigma^{-1} \mu \end{align} }$