C++でのEKF SLAMについて

質問

Visual C++ 2008で EKF SLAMのシミュレーションを行っております．
「確率ロボティクス（http://www.amazon.co.jp/%E7%A2%BA%E7%8E%87%E3%83%AD%E3%83%9C%E3%83%86%E3%82%A3%E3%82%AF%E3%82%B9-ROBOT-books-Sebastian-Thrun/dp/4839924015）」

に書かれているEKF SLAMアルゴリズムの項を参考に
プログラムを組んでいますがどうにもうまくいきません．

具体的には，ロボットが新しいランドマークを認識したとたん，
ロボット姿勢の推定値が狂い，はるか彼方へ飛んで行ってしまいます．

記述とは何度も突き合わせてみたのですが，間違っているところが見つからず困っております．
ソースコードを含むプロジェクトフォルダを以下にアップロードしますので，よろしければ
ご指摘くださると幸いです．
（表示にOpenCV2.2を使っています．またダウンロードパスは"pass"です）
http://www5.puny.jp/uploader2/download/1307532699.zip

回答No.1

LEVELUP100

こんばんは。

　あまりプログラムのことは詳しくないので助言は不可能ですが、試すとしたら誤植情報が出版社から提供されていないか確認してみるのはどうでしょう？

お礼

ありがとうございます
確認してみたところ，誤植に関する情報はないようです．