確率ロボティクス輪読会 3章
•Download as PPTX, PDF•
2 likes•713 views
確率ロボティクス2～3回目の資料、KFとEKF以外はかなり端折ってる
Recommended
More Related Content
Featured
Featured (20)
確率ロボティクス輪読会 3章
- 3. カルマンフィルタ • Gauss分布で信念を表現し、媒介変数として平均及び共分散を 用いる • 前提条件(満たされてれば信念がGauss分布となる) • 状態遷移確率及び計測確率は線形Gauss型モデル • 初期信念がGauss分布
- 4. 線形ガウス型モデル 例えば状態遷移確率の場合に成立するのは 𝑥 𝑡 = 𝐴 𝑡 𝑥 𝑡−1 + 𝐵𝑡 𝑢 𝑡 + 𝜖 𝑡 即ち線形モデル+Gauss雑音(Gauss分布に従う雑音) 平均:𝐴 𝑡 𝑥 𝑡−1 + 𝐵𝑡 𝑢 𝑡 分散:𝑅𝑡(𝜖 𝑡の分散) 平均と分散が定義できるので𝑝(𝑥 𝑡|𝑥 𝑡−1, 𝑢 𝑡)がGauss分布になる 計測確率の場合zt = 𝐶𝑡 𝑥 𝑡 + 𝛿𝑡となり同様に𝑝(𝑧𝑡|𝑥𝑡)がGauss分布に
- 5. カルマンフィルタのアルゴリズム 1. Algorithm Kalman_filter( mt-1, St-1, ut, zt): 2. Prediction: 3. 4. 5. Correction: 6. 7. 8. 9. Return mt, St ttttt uBA 1mm t T tttt RAA SS 1 1 )( SS t T ttt T ttt QCCCK )( tttttt CzK mmm tttt CKI SS )( ベイズフィルタの式に線形ガウス型モデ ルぶち込めば出てくる ベイズフィルタの式に(ry カルマンゲイン、次ページ
- 6. カルマンゲイン 𝐾𝑡 = Σ 𝑡 𝐶𝑡 𝑇 𝑄𝑡 −1 • 信念の共分散Kt, 計測確率の平均𝐶𝑡及び共分散𝑄𝑡の積 • 要はどれくらい計測を信念に反映させるか • と言うとユーザーサイドで適当に決めていいかのように聞こえ るが計測確率分布からスッとと決まるやつ
- 7. カルマンフィルタの線形化 • 実際の状態遷移モデル及び計測モデルが線形であることは少ない • 例えばモータの応答が非線形だったり • 非線形だとこんな感じにモデリングされる 状態遷移確率: 𝑥 𝑡 = 𝑔 𝑢 𝑡, 𝑥 𝑡−1 + 𝜖 𝑡 計測確率:𝑧𝑡 = ℎ 𝑥 𝑡 + 𝛿𝑡 • しかし非線形だと扱いがめんどい→線形近似 • Taylor展開による近似→拡張カルマンフィルタ(EKF) • アンセンテッド変換による近似→アンセンテッドカルマンフィルタ(UKF)
- 8. 拡張カルマンフィルタ(EKF) Taylor展開 𝑓 𝑥 = 0 𝑛 1 𝑛! 𝑓 𝑛 𝑎 𝑥 − 𝑎 𝑛 ≅ 𝑓 𝑎 + 𝑓′ 𝑎 𝑥 − 𝑎 非線形関数を線形近似(Taylor展開を1次で切る) 𝑔 𝑢 𝑡, 𝑥 𝑡−1 = 𝑔 𝑢 𝑡, 𝜇 𝑡−1 + 𝑔′(𝑢 𝑡, 𝜇 𝑡−1 )(𝑥 𝑡−1 − 𝜇 𝑡−1) ℎ 𝑥 𝑡 = ℎ 𝜇 𝑡 + ℎ′(𝜇 𝑡)(𝑥𝑡 − 𝜇 𝑡) 0次 𝐺𝑡(ヤコビ行列) 1次 𝐻𝑡
- 9. EKFアルゴリズム 1. Extended_Kalman_filter( mt-1, St-1, ut, zt): 2. Prediction: 3. 4. 5. Correction: 6. 7. 8. 9. Return mt, St ),( 1 ttt ug mm t T tttt RGG SS 1 1 )( SS t T ttt T ttt QHHHK ))(( ttttt hzK mmm tttt HKI SS )( 1 1),( t tt t x ug G m t t t x h H )(m ttttt uBA 1mm t T tttt RAA SS 1 1 )( SS t T ttt T ttt QCCCK )( tttttt CzK mmm tttt CKI SS )( それっぽくKFから置換されている ことを認識してほしい
- 10. ぶっちゃけると • ガウシアンフィルタ使う場合だいたいEKF (ex)日産の自動運転車研究、hector_slam(ROSのSLAMパッケージ) • 利点:計算が早い • 計算量オーダー:𝑂 𝑘2.4 + 𝑛2 • 欠点:ユニモーダルである • 2つ山作って近似したいけどユニモーダルだからできないよねという話→ • 対策:ガウス分布の和を信念とする(複数仮説カルマンフィルタ)
- 11. アンセンテッドカルマンフィルタ(UKF) • Taylor展開より推定が正確になる線形近似手法 • モーメントマッチング • アンセンテッド変換←こいつを使おう • シグマ点の導入 • 分布の中心と中心に対称な各軸上の点2つ(次元毎、距離は人間が選ぶ) • つまり1+2n(nは次元)個選ばれる • シグマ点をgやhで変換したものに重み付けして足し合わせる • ぶっちゃけ使わない可能性が高いので式は各自追って(ごめん)
- 12. EKFvsUKF • 計算が速いのはEKF(ただし定数倍なのでオーダーは一緒) • 正確性はUKFの方が上(ただし分布の形状次第) • UKFの場合微分が不要なので計算機によっては利点になる
- 13. 情報フィルタ(IF) • カルマンフィルタでは平均と分散でガウス分布を表現した • 情報フィルタでは情報行列と情報ベクトルで表現する • 情報行列Ω:共分散行列Σの逆行列(Σ−1) • 情報ベクトル𝜉 = Σ−1 𝜇 • ガウス分布をシンプルに表現できてエレガント(らしい) 𝑝 𝑥 = 𝜂exp{− 1 2 𝑥 𝑇Ω𝑥 − 𝑥 𝑇 𝜉} • カルマンフィルタと同様信念は線形ガウス型モデル • 状態遷移及び計測更新毎にΩ, 𝜉, Ω, 𝜉を更新していく • カルマンフィルタと数学的に双対