Recommended
More Related Content
What's hot
What's hot (20)
Similar to カルマンフィルタ入門
Similar to カルマンフィルタ入門 (20)
カルマンフィルタ入門
- 1. 2021.07.15 二瓶 泰徳 株式会社ディー・エヌ・エー ＋ 株式会社 Mobility Technologies カルマンフィルタ入門
- 2. 2 自己紹介 名前 二瓶 泰徳 経歴 自動車メーカー等でプロジェクトマネジメント => データサイエンティスト 現在のお仕事 Mobility Technologiesのデータサイエンスグルー プに所属し、プロダクトのデータ解析、機能開発 等を担当 趣味 スノーボード、登山、キャンプ、 スキューバダイビング
- 4. 4 参考文献 [1] Kalman and Bayesian Filters in Python [2] カルマンフィルタの基礎 今回紹介する実験は全て[1]のjupyter notebookをベースとしています
- 9. 9 𝒙𝒕は𝒙𝒕−𝟏から算出するが、 𝒙𝒕と𝒙𝒕−𝟏の関係をモデル化しきれない部分が誤差と して残る 位置センサの観測精度には限界があるため、 𝒛𝒕にも誤差が含まれる 誤差を含む2つの量(𝒙𝒕と𝒛𝒕)を統合して、 𝒙𝒕を求めたい カルマンフィルタが適応できる問題例 𝒙𝒕−𝟏 𝒛𝒕 𝒙𝒕 𝒙𝒕
- 11. 11 • 先程の例ではスカラ量を扱ったが、多次元に拡張可能 • 状態空間と観測空間の変換がモデル化できれば𝒙と𝒛は異なる次元でもよい。 • カルマンフィルタは、以下の仮定を前提としている: 補足 • 𝒙𝒕−𝟏 と 𝒙𝒕 及び 𝒙𝒕と 𝒛𝒕 の関係が線型結合で表現できる • 状態誤差と観測誤差が白色ガウス雑音
- 12. 12 02 アルゴリズムの解説
- 13. 13 カルマンフィルタのアルゴリズム Predict Update 𝐱, 𝐏 : 状態推定値及び誤差共分散 : 事前状態推定値及び事前誤差共分散 : 状態遷移モデル : 制御入力 : 観測モデル : 状態遷移誤差共分散 I : 観測値及び観測誤差 : Innovation : カルマンゲイン : 単位行列 𝐱, 𝐏 *数式の表現は[1]に準ずる
- 14. 14 Predict – 状態推定量の計算 一つ前の状態から、現在の状態を推定する 例えば状態が車の位置と速度を示す場合 𝐅𝐱は、 と表現できる。 Buは制御入力を表す。制御入力がない場合は省略できる。 𝐱, 𝐏 : 状態推定値及び誤差 :事前状態推定値及び事前誤差共分散 : 状態遷移モデル 𝐱, 𝐏
- 15. 15 Predict – 状態誤差の計算 𝐱, 𝐏 : 状態推定値及び誤差共分散 : 状態遷移モデル : 状態遷移誤差 𝐱, 𝐏 1つ前の状態誤差から、現在の状態誤差を推定する 𝐐は状態遷移により発生する誤差を表す。(遷移行列𝐅でモデル 化出来なかった部分がここに乗るイメージ) 𝐅𝐏𝐅𝐓 とすることで、状態量の各要素の相関性を考慮に入れるこ とができる :事前状態推定値及び事前誤差共分散
- 16. 16 状態量が物体の位置、速度の場合の計算例 対角成分はそれぞれ分散 対角成分以外は状態変数同士の共分散を示す。 共分散は事前知識があれば数字を入れてしまえばいいが、わ からなければ相関がないとすれば良い 位置と速度は 𝑥 = 𝑥0 + 𝑣𝑑𝑡の関係で記述できることから、 今回の例では位置、速度の誤差分散に相関があると考えら れる。 左の式のように、 𝐅𝐏𝐅𝐓 の計算で共分散の成分もアップデー トされている これに𝐐を加えて𝐏を更新する Predict –状態誤差の計算 𝐱, 𝐏 : 状態推定値及び誤差共分散 : 状態遷移モデル
- 17. 17 Update –Innovationの計算 : Innovation : 観測値及び観測誤差 : 事前状態推定値及び事前誤差共分散 𝐱, 𝐏 : 観測モデル 観測値と事前状態推定値の差 観測値と事前状態推定値は次元が異なるため、 𝐇にて変換している
- 18. 18 とおくと、 カルマンゲイン𝐊は𝐒を用いて と、表現できる。逆行列を行列同士の割り算と考え ると、 Update –カルマンゲインの計算 右辺の逆行列を 𝐒は状態誤差と観測誤差の和で表現される 右辺第1項が𝐇𝐏𝐇𝐓 なのは状態空間から観測空 間への変換のため : 観測値及び観測誤差 : 事前状態推定値及び事前誤差共分散 𝐱, 𝐏 : 観測モデル
- 19. 19 観測誤差が大きい -> 予測値を重視 予測誤差が大きい -> 観測値を重視 観測誤差が大きい -> (𝐈 − 𝐊𝐇)が大きい -> 𝐏が大きく 予測誤差が大きい -> (𝐈 − 𝐊𝐇)が小さい -> 𝐏が小さく Update – 𝐱, 𝐏の更新 𝐱の更新 𝐏の更新 : Innovation :事前状態推定値及び事前誤差共分散 𝐱, 𝐏 : 観測モデル 𝐱, 𝐏 : 状態推定値及び誤差共分散
- 21. 21 アルゴリズムの例題への適用 決めなければならないこと: • 𝐱, 𝐏の初期値 • 𝐅, 𝐐 • 𝐇 • 𝐳, 𝐑 * 今回は制御入力を考えないので、𝐁, 𝐮は省略 直線上を1m/s + 誤差 で進む物体の位置及び速度を推定する 物体は原点から出発するとする 観測できるのは物体の位置のみ
- 22. 22 パラメーターの設定 𝐱, 𝐏の初期値 ドメイン知識があれば上手く利用して設定する。 実問題に適用する場合は、 𝐱は観測値の初期値に基づき設定することが多い 今回は以下の通り設定 𝐱 = 𝑥 𝑣 = 10 4.5 𝐏 = 𝜎𝑥 2 𝜎𝑥𝑣 𝜎𝑥𝑣 𝜎𝑣 2 = 500 0 0 49
- 23. 23 パラメーターの設定 単純な等速直線運動なので、 𝐅は以下のように表現できる 𝐐の設定は様々な手法が提案されているが、Piecewise White Noise Modelと呼ばれる手法を 適用すると、 𝐐は以下のように設定できる(詳細は省略)。 𝐅 = 1 ∆𝑡 0 1 𝐐 = ∆𝑡4 4 ∆𝑡3 2 ∆𝑡3 2 ∆𝑡2 𝜎2 ここで、𝜎2 は遷移誤差分散を示す。今回は 𝜎2 = 0.1と設定する 𝐅, 𝐐の設定
- 27. 27 アルゴリズムの例題への適用 𝐑を大きく設定した状態で、 𝐐を変化させて挙動を確認する 𝐐 が大きい => 状態推定値より測定値を信頼する => KFの出力は測定値寄りになる 𝐐 が小さい =>測定値より状態推定値を信頼する => KFの出力は測定値の変化に対して鈍 感になる
- 28. 28 アルゴリズムの例題への適用 𝐑をさらに大きくして、 𝐱の初期値を変化させる 𝐑 が大きいため、測定値より推定値を重視し、 なんとか真の値に近づこうとしている Rが大きい -> 状態推定値を重視する -> 状態 の初期値が外れていると、いつまでも収束し ない 𝐱 = 0 1 𝐱 = 50 1
- 29. 29 アルゴリズムの例題への適用 ステップを重ねるごとに推定値が収束して いくのが確認できる 𝐑 = 225𝑚2 として、 𝐱の初期値が真の値から外れている場合の 推定値の遷移を確認する 𝐱 = 20 1
- 30. 30 アルゴリズムの例題への適用 推定値から速度を除外するとどうなるか？ 1D Filterは速度(=位置の変化量)を知る術がないので、真 の値を数ステップ遅れて追従するような挙動を取る 2D Filter : 推定値は物体の位置と速度 1D Filter : 推定値は物体の位置
- 31. 31 アルゴリズムの例題への適用 𝐏の推移を確認する 初期値: 𝐏 = 1.5 0 0 0.3 ステップが進む毎に、位置と速度の相関性が反映されて、 斜めに長い楕円形状になる
- 33. 33 パフォーマンスの評価 真の値が分からない場合 真の値が分かる場合(シミュレーションなど) Normalized estimated error squared (NEES) を使う 値が小さい方ど性能が良い 設定した仮定(線形性、誤差の分散など)に対するフィルタの出力の尤度 または対数尤度を計算する 尤度が小さければ、設定した仮定のどこかが間違っている 𝐱 : 状態推定量 𝐱 : 状態の真の値
- 37. 37 EOF