Kalman filter class for Newtonian 2 dimentional world.
原理:
運動モデルは以下で表されるとする。
ただし, は誤差, 外乱。 この式を使って予報値
を求めるとする。すなわち,
この時, 予報誤差共分散行列 は、
で変化する。 ただし, は
の共分散行列。
観測値
が得られたときには、 以下のようにデータを統合して推定値を求める。
仮に,
は観測行列
を用いて以下のように求まるものとする。
また、 は
の共分散行列とする。 これらを用いて, まずカルマンゲイン
を求める。
これを用いて,以下のように推定値 を求める。
この時, 推定誤差共分散行列 は、
この ,
を新たに
,
として、次の予報を行う。
2次元ニュートン運動方程式における実装:
状態変数
は, 位置の x-y, 速度の x-y, 加速度の x-y の6次元あるものとする。 この時,
は以下のような行列とする。
また、観測行列 は、位置だけが観測できるものとする。
また、各誤差共分散行列は, 対角成分のみとする。 使い方:
- 予測
- kfilter.vXf() に現在値を代入。
- kfilter.wPf() に現在値を代入。
- kfilter.estCycle() を呼び出す。
- kfilter.wPf() を格納
- kfilter.vXf() から状態変数値を取り出す。
- 観測値統合
- kfilter.vXf() に現在値を代入。
- kfilter.wPf() に現在値を代入。
- kfilter.mergeSensor(pos_x,pos_y) を呼び出す。
- kfilter.wPf() を格納
- kfilter.vXf() から状態変数値を取り出す。
線型力学系を記述する行列
wFt の対称行列
予報誤差共分散
予報値誤差共分散
サンプリングフィルタ
センサー誤差共分散
カルマンゲイン
合成推定値誤差共分散行列
予報値
テストで用いる真の値
センサー値
予報値とセンサー値から補正された推定値
パラメータの初期値をデフォルトに設定。
パラメータの初期値をデフォルトに設定。
パラメータに従ってマトリクスの初期値を設定
線型式に基づき変化の予測と各種パラメータ推定を行う。
センサー情報を統合する。
テストのための外界のシミュレーション
testEnvCycle() を使って, Kalman Filter のテストをする1サイクル
testCycle() を呼び出す。
Alphabetic index HTML クラス階層 or Java