はじめに
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
状態空間モデル
状態モデル（状態方程式）
xt=F⋅xt−1+B⋅ut−1+qt−1=[1000010000100000][xt−1yt−1ψt−1vt−1]+[Δt⋅cosψt−10Δt⋅sinψt−100Δt10][vt−1ψt−1]+qt−1
観測モデル（観測方程式）
zt=H⋅xt+rt=[10000100][xtytψtvt]+rt
予測ステップ
χt−1=(xt−1,xt−1+γ√∑t−1,xt−1−γ√∑t−1)ˉχ∗t=g(ut,χt−1)ˉxt=2n∑i=0w[i]mˉχ∗[i]tˉ∑t=2n∑i=0w[i]c(ˉχ∗[i]t−ˉxt)(ˉχ∗[i]t−ˉxt)T+Rtˉχt=(xt,xt+γ√∑t,xt−γ√∑t)ˉZt=h(ˉχt)ˆzt=2n∑i=0w[i]mˉZ[i]tSt=2n∑i=0w[i]c(ˉZ[i]t−ˆzt)(ˉZ[i]t−ˆzt)T+Qtˉ∑x,zt=2n∑i=0w[i]c(ˉχ[i]t−ˉxt)(ˉZ[i]t−ˆzt)T
更新ステップ
Kt=ˉ∑x,ztS−1txt=ˉxt+Kt(zt−ˆzt)∑t=ˉ∑t−KtStKTt
参考文献
Probabilistic Robotics/Sebastian Thrun, Wolfram Burgard and Dieter Fox.
ロボット・ドローン部品お探しなら