VSLAMの実装を行う。練習としてEKF SLAMに取り組む。GSLの行列や乱数などを利用してシミュレータを作る。VTKで結果を表示する。
SLAMの前にやったEKF位置推定からわかったこと: 信念から予測を行ったら、得られた観測すべてについて更新ステップを行う。一度に反映するのは1つだけでも、次の予測の前に更新し続けることですべての観測を反映できる。
相関のあるノイズの作り方について: 確率変数Xの分散共分散行列Var(X)が対角成分しか含まないとき、行列AをかけたAXの分散共分散行列は、A Var(X) A' となる。相関のない乱数からなるベクトルに対し、適当な行列をかけてやれば、成分間に相関ができる。手間はかかるけど一応相関のあるノイズは作れる。
