なんとなくわかってきたと思ったら次の章で倍々にモヤモヤが増々していくカルマンフィルタを写経で成仏させていくシリーズ。
前回は以下のように多変量カルマンフィルタが成仏した。
引き続き、以下ドキュメントの無香料カルマンフィルタに関するPythonプログラムをRustで写経し成仏させていく。
著者いわく「EKFを実装したことがあるなら、UKF はなんて簡単なんだ」と書いてあったので簡単なんだと期待して始めたのだが...難しいじゃん！特に実装が。
てなわけでなんとか動くものは作れたが、何点か制約ができてしまった。
①シグマポイントのα設定を0.07より小さくするとコレスキーが死ぬ。
②ランドマークが見えない場合の対応が出来ない。
③シグマポイントはMerweScaledのみ。
①については設定値と回数に依存するっぽい。精度の問題なのか実装がどこかおかしいか解らんまま。
atan2が怪しいような、もしくは剰余計算時の雑な扱いのせいか...
以下のお方がおっしゃるようにSVDでやればいいじゃんと思って１度実装してみたのだが、型引数とデフォルトアロケータの扱いがめちゃ増えてしまって手に負えず断念した。
②についてはドキュメントの例題もランドマークが見えない場合のパターンが無いように見え、どうしたらいいか解らん。
③についてはトレイト使ってシグマポイントの入れ替えを可能にするだけといえばそうなのだが、型引数の対応をさらに行わなければならず断念した。
あとは10.8 複数の位置センサーの航空機位置をかえたときの結果がドキュメントと違って大幅にズレている。
これも解らんまま。
出来たコードは型引数とデフォルトアロケータの記述でおなかいっぱい感があると思うがそこは慣れなので...
やっつけフェーズとは
発生した問題を常に先送りし、周囲からの信頼や成長の機会を失いないながらも、動くモノが出来てそれっぽく動作すればオッケーなフェーズ。
無香料カルマンフィルタ
出来上がり
コード
- us_kalmanfilter
無香料カルマンフィルタ - ms_sigmapoints
MerweScaledシグマポイント - us_transform
無香料変換
ここから以下は無香料カルマンフィルタexampleプログラム。
- draw-robot-sim-ukf
10.7 UKFを使う - draw-radaracc-sim-ukf
10.8 航空機の追跡 - radaraccsim
航空機のシミュレータ - draw-radaracc-cv-sim-ukf
10.8 機動する航空機の追跡 - draw-sensor-without-doppler
10.8 センサー統合:ドップラー未使用 - draw-sensor-with-doppler
10.8 センサー統合:ドップラー使用 - draw-sensor-multiple
10.8 複数の位置センサー - draw-self-positioning
10.15 ロボットの自己位置推定─完全な例 - robotukf/robotukffn
上記10.15のfxやhxなどの関数定義を切り出し。 - robotukf/robotukfsim
上記10.15の運動モデル、制御入力、シミュレーション全般の設定を切り出し。
このようにして無香料カルマンフィルタが90%成仏した。
以上
