SLAM～Unscentedカルマンフィルタ

はじめに

UKF(Unscented kalman filter)では無損変換(Unscented Transform)の線形化手法を利用し、つまり(2*n次元+1)個Sigma Pointの線形回帰に基づき、確率変数の非線形関数を線形化する。確率変数の拡張を考慮するので、この線形化は拡張カルマンフィルタEKFで使用するテイラー級数線形化より比較的正確に状態推定できる他、EKFと違い、状態・観測ヤコビアン行列を求める必要でなく、但しEKFと同様にUKFも予測と更新の手順から時系列の状態推定を行う。状態変数、観測変数ともガウス分布に当てるのがカルマンフィルタKFの共通特徴という。またガウス分布に当てないパーティクル(和文粒子、英文Particle)フィルタPFがある。そもそもガウス分布に従う確率変数の非線形関数はガウス分布にならないから、EKF、UKFよりPFの方が精度よく、ROSのSLAM Gmappingアルゴリズムに組まれている。

Unscented Kalman Filter

状態空間モデル

状態モデル（状態方程式）
$$x_t = F \cdot x_{t-1} + B \cdot u_{t-1} + q_{t-1} \\\scriptsize=\begin{bmatrix}1&0&0&0\\0&1&0&0\\0&0&1&0\\0&0&0&0\end{bmatrix}\begin{bmatrix}x_{t-1}\\y_{t-1}\\ψ_{t-1}\\v_{t-1}\end{bmatrix}+\begin{bmatrix}Δt\cdot cosψ_{t-1}&0\\Δt\cdot sinψ_{t-1}&0\\0&Δt\\1&0\end{bmatrix}\begin{bmatrix}v_{t-1}\\ψ_{t-1}\end{bmatrix}+q_{t-1}$$

観測モデル（観測方程式）
$$z_t = H \cdot x_t + r_t \\=\begin{bmatrix}1&0&0&0\\0&1&0&0\end{bmatrix}\begin{bmatrix}x_t\\y_t\\ψ_t\\v_t\end{bmatrix}+r_t$$

予測ステップ

$$χ_{t-1}=(x_{t-1} , x_{t-1}+γ\sqrt{{\sum}_{t-1}} , x_{t-1}-γ\sqrt{{\sum}_{t-1}})\\
\bar{χ}_t^{*}=g(u_t,χ_{t-1})\\
\bar{x}_t=\sum_{i=0}^{2n} w_m^{[i]} \bar{χ}_t^{*[i]}\\
\bar{\sum}_t=\sum_{i=0}^{2n} w_c^{[i]}(\bar{χ}_t^{*[i]}-\bar{x}_t)(\bar{χ}_t^{*[i]}-\bar{x}_t)^T +R_t\\
\bar{χ}_{t}=(x_{t} , x_{t}+γ\sqrt{{\sum}_{t}} , x_{t}-γ\sqrt{{\sum}_{t}})\\
\bar{Z}_{t}=h(\bar{χ}_{t})\\
\hat{z}_{t}=\sum_{i=0}^{2n} w_m^{[i]} \bar{Z}_{t}^{[i]}\\
S_t=\sum_{i=0}^{2n} w_c^{[i]}(\bar{Z}_t^{[i]}-\hat{z}_t)(\bar{Z}_t^{[i]}-\hat{z}_t)^T +Q_t\\
\bar{\sum}^{x,z}_t=\sum_{i=0}^{2n} w_c^{[i]}(\bar{χ}_t^{[i]}-\bar{x}_t)(\bar{Z}_t^{[i]}-\hat{z}_t)^T
$$

更新ステップ

$$K_t=\bar{\sum}^{x,z}_t S^{-1}_t\\
x_t = \bar{x}_t+K_t(z_t-\hat{z}_t)\\
{\sum}_t = \bar{\sum}_t-K_t S_t K^T_t$$

参考文献

Probabilistic Robotics/Sebastian Thrun, Wolfram Burgard and Dieter Fox.