Rustやっつけフェーズ(11)カルマンフィルタ②1次元カルマンフィルタ

Rust wio-terminal カルマンフィルタ

カルマンフィルタ成仏シリーズとして開始した前回は以下のように離散ベイズフィルタが成仏した。

今回も前回から引き続き、以下ドキュメントの1次元カルマンフィルタに関するPythonプログラムをRustで写経し成仏させていく。

この後に控える多変量カルマンフィルタを前に、1次元のガウス分布である平均と分散を用いて、事前分布と観測値の残差を求め、その間での次の予測値を求めるパーセンテージを計算し、そのパーセンテージ(これをカルマンゲインと呼ぶ)に従い次の予測値(事前分布)を決定することを学ぶ。

前回の離散分布はドアが10個あれば、10個分の変数か必要だったが、ガウス分布を用いるとその釣鐘状の形態によって１つの平均と１つの分散で事足りて計算量が格段に経済的になる。
ただしその分制限があるというものらしい。

正しい説明はできるはずもないのでこんな解釈で進める。正しい内容を知りたければ上記ドキュメントを参照のこと。

また、事前分布予測→観測値尤度計算→事後分布への更新の繰り返しを行って行くと分散値はある値に収束して行き、分散値が収束するとカルマンゲインも収束する。
で、実験で収束したカルマンゲインを最初から固定値として用いると精度は良いままで計算量がこれまた節約できるというシロモノであるそうだ。

やっつけフェーズとは

なんとかのひとつ覚えのみで乗り切ろうをモットーに、車輪のダウングレード再発明をデフォとして、動くものが出来ればオッケーなフェーズ。

1次元カルマンフィルタ

出来上がり

コード

  • one_dimensional
    1次元カルマンフィルタ関数ライブラリ
    • ガウス分布構造体の定義
    • フィルタ関数:predict(予測)、update(更新)のトレイト定義
    • 上記トレイトを実装した3種類のオブジェクト。
      • ガウス分布の積(尤度x事前分布)パターン
      • カルマンゲイン算出パターン
      • 固定ゲイン設定パターン
    • シミュレーショントレイトの定義
  • dogsimulation
    犬の動きのシミュレーション。電圧のシミュレーションもこいつで事足りたので「dog」は余計だったが仕方ない。

  • nonlinersimulation
    非線形系のシミュレーション

  • od_kalmanfilter 1次元カルマンフィルタ操作オブジェクト

    • 生成時にフィルタ関数オブジェクトとシミュレーションオブジェクトを受け取り保持する。
    • セッターにてプロセスモデル、観測ノイズ分散、初期推定値、移動量などを設定する。
    • iterationsメソッドにて1ステップ分の以下動作を行う。
      • シミュレータを1ステップ動かし観測値を取得。
      • 事後分布、プロセスモデルから事前分布を予測。
      • 観測値、センサ誤差から尤度（もっともらしさ）を設定。
      • 尤度、事前分布から事後分布を計算・更新。
      • 指定により初期観測値を初期推定値に設定可能。

ここから以下の「draw-〜」は1次元カルマンフィルタオブジェクトを生成、設定し画面表示を行うexampleプログラム。

  • draw-dogsim
    4.6 初めてのカルマンフィルタ
  • draw-dogsim-rest
    上記のカルマンゲイン算出バージョン
  • draw-dogsim-var
    カルマンフィルタのアニメーション
  • draw-voltsim
    4.11 カルマンフィルタの設計入門
  • draw-voltsim-var
    上記のアニメーション
  • draw-dogsim-bigsensornoize
    4.12 極端に大きい観測ノイズ
  • draw-dogsim-smallprocessnoize
    4.13 極端に小さいプロセス分散
  • draw-dogsim-wronginit
    4.14 大きく間違った初期推測値
  • draw-dogsim-bignoize_wronginit
    4.15 大きいノイズと大きく間違った初期推測値
  • draw-dogsim-bignoize_zinit
    最初の観測値を初期推測値にする
  • draw-dogsim-nonliner
    4.17 非線形な系

ここから以下の「emb-〜」はグラフ表示ライブラリ

以下はテスト用プログラム

このようにして1次元カルマンフィルタが成仏した。
以上