最近，全然ブログの更新をしていないのに何故か毎月アクセス数が増えていて，なんだか申し訳ない気持ちになったので久々の更新です．アクセス数の多いページの傾向から察するに，かなりマニアックな人たちが見に来てくれている（このへんの記事[1][2][3]のアクセスが多い）ようなので，今後も一般人には媚びず，マニア向けの(?)内容を書いていこうかと思います．

ということで今回は非線形カルマンフィルタの一種のUnscented Kalman Filterを実装しました．
UKFでは状態の正規分布の平均周りの数点（シグマ点と言います）をサンプリングしてきて，それらの点を実際にシステムを表す非線形関数で変換してやり，変換後の点列から平均・分散を計算することで状態の更新を行います．ヤコビアンの計算が必要ない上に，共分散の変換も二次近似になるため推定精度が良くなるといういいこと尽くめです．詳細は山北先生の論文[1]をどうぞ．丁寧な説明でわかりやすいです．
以前にIMUを使った姿勢推定でも使っていましたが，今回はpython用に書き下してみました．ソースを置いておくので欲しい人は持っていってください．

続きを読む →