カルマンフィルタわかったようでわからん。そんなモヤモヤを成仏させるシリーズとして前回は以下のように1次元カルマンフィルタが成仏した。
今回も前回から引き続き、以下ドキュメントの多変量カルマンフィルタに関するPythonプログラムをRustで写経し成仏させていく。
今回から多変量ということで行列演算が発生する。
なのでまたまたnalgebraの登場となるのだが、no_stdであるためダイナミックMatrixは使えそうな気がしない。
で、任意の行列次元を、関数を利用する側から指定するのってどうしたらええの？って探したところ以下の記事がものすごく参考になった。
3年前の記事。すごい。天才。
いつも思うのだが、このお方達はどのようにしてこのような技を習得するのだろうか...
やっつけフェーズとは
なんとかのひとつ覚えのみで乗り切ろうと思ってはいるのだが、それだけではどうにもならないこともあるもので、とりあえずそのような技を拝借しつつも動くものが出来ればオッケーなフェーズ。
多変量カルマンフィルタ
出来上がり
コード
- mv_kalmanfilter
多変量カルマンフィルタ関数ライブラリ- ジェネリクスで各行列の次元を利用側から指定可能。
- セッター/ゲッターは廃止し、各行列をまるっとpublic。
利用側から設定/取得。
- discrete_white_noise
離散白色ノイズ- ノイズのブロック分のみ作成。
- 利用側の変数型定義により、2x2 or 3x3 or 4x4 のノイズブロックを作成。
- block_diag的なことは利用側で行う。
- robot2d
xy平面を動くロボットのシミュレータ - constantacc
定常加速シミュレータ
ここから以下の「draw-〜」は多変量カルマンフィルタオブジェクトを生成、設定し画面表示を行うexampleプログラム。
- draw-dog-sim-mv
6.6 カルマンフィルタの実装 - draw-dog-sim-1d2d
6.9 隠れ変数の効果 - draw-dog-sim-ellipses
6.12共分散行列の詳しい調査 - draw-robot-sim-ellipses
8.1 ロボットの追跡- ここではブロックノイズをblock_diag的なことをして、プロセスノイズに設定する。
- draw-acc-sim
8.3 フィルタの次数の評価
ここから以下の「emb-〜」はグラフ表示ライブラリ
- emb_covargraph
共分散円グラフ
以下はテスト用プログラム
- wio-draw-covargraph
共分散円グラフテスト - debug_mvkalmanfilter
ターミナル確認用
このようにして多変量カルマンフィルタが成仏した。
以上
