色々な事情があり，使用するロボットの台数が4台から2台へ変更になったため，今回はそれを反映させました．

また，画像からロボットの位置と姿勢を算出した際，ノイズがのることで状態FBが上手くかからなくなることの対策としてUKF(非線形カルマンフィルタ)を実装したのでその結果をUPしておきます．

上記のアニメーションを行った際の推定結果が以下がです．
今回は真値に対して正規分布に従う乱数を加えることでノイズを表現しています．

結構大きめのノイズをのせてみましたが，真値に近い値が推定できている事がわかります．

それぞれの状態量の推定結果が以下です．

X,Yそれぞれで見るとあまり大きなノイズに見えませんが，XY座標上にプロットしてみると真値から外れている様子がわかります．

デスクトップPCで計算できること，ロボットの台数が減ったことから計算時間には大分余裕がありそうなので，UKFの状態量点列を変換する際に単純なオイラー法ではなく4次のルンゲクッタで計算するようにしているので推定値の制度は大分良さそうです．
(実際はこんなにノイズのらないと思うし)

次はロボット同士の衝突回避を実装しようと思います．