概要
SLAM（ロボットの自己位置推定、マッピング同時に行うこと）に使用可能なセンサは、色々とあるが、それぞれ単一のセンサでは完璧ではないため、複数のセンサ情報を、統計的・確率的に組み合わせて、より精密で安定した自己位置を得る手法が一般的だ。状態値（位置、姿勢またはポーズ）、観測値とも正規分布（ガウス分布）に近似可能の場合、カルマンフィルタKFが適用される。また、非線形問題に対して、拡張カルマンフィルタEKFが適用される。勿論、UKF他のフィルタが存在する。なぜかROSに公式に採用されたのはEKFのようだ。ここでは、タイヤパルスセンサ（ホール式や、光学式エンコーダ）からのodom情報と距離センサ(レーザや、カメラなど)からの位置情報を組み合わせて、マッピング地図がある前提でロボットの自己位置、姿勢に拡張カルマンフィルタをかけて推定してみる。
状態空間モデル
xt=ft−1(xtruet−1+pt−1)+qt−1=[xt−1+vt−1⋅Δt⋅cosψt−1000yt−1+vt−1⋅Δt⋅sinψt−1000ψt−1+ωt−1⋅Δt]+qt−1
yt=Ht(xtruet)+rt=[xtyt]+rt
拡張カルマンフィルタの適用
予測ステップ
事前状態推定値
ˉxt=[ˆxt−1+vt−1⋅Δt⋅cosˆψt−1000ˆyt−1+vt−1⋅Δt⋅sinˆψt−1000ˆψt−1+ωt−1⋅Δt]
線形化近似（ヤコビアン行列）
ˆFt=∂ft(x)∂x|x=ˆxt−1=[10−vt−1⋅Δt⋅sinˆψt−101vt−1⋅Δt⋅cosˆψt−1001]
事前誤差共分散行列
¯Pt=ˆFt−1ˆPt−1ˆFTt−1+Qt−1
フィルタリングステップ
カルマンゲイン行列
Kt=¯PtHTt(Ht¯PtHTt+Rt)−1
状態推定値
^xt=¯xt+Kt(yt–Ht¯xt)
事後誤差共分散行列
^Pt=(I−KtHt)¯Pt
関連記事
9軸IMUセンサ 6軸／9軸フュージョン 低遅延 USB出力 補正済み ROS対応
研究開発用 台車型ロボット キット
ロボット・ドローン部品お探しなら