カルマンフィルタ

第１４回ロボット⼯学 カルマンフィルタ

ルドルフ・カルマン １９３０年ハンガリのブタペスト⽣まれのア メリカの学者（２０１６年没） １９６０年、⼤学院の時にカルマンフィルタ を考案する。 アポロ宇宙船の軌道推定に使⽤され，その実 ⽤性が⾼く評価される。 Wikipediaより転載 現代制御理論の⽣みの親。

ロボットも宇宙船もまっすぐ⾏きたいと 思っていても、太陽光による熱による推 進装置の歪みや燃料噴射の不均⼀、路⾯ の摩擦の局所的な違いによって予定通り には進めない

左行け ロボットも宇宙船もこういう軌跡を描 いて動いて欲しいという要求があり、制 御する必要がある。 そのため、位置を観測する必要がある。 しかし、観測にも観測の誤差が必ずあ り本当の位置はわからない。 右行け

動きが思う様にいかないこと は、いつどのくらいになるか は確率的な現象。 観測した時に含まれている誤 差もいつどのくらいになるか は確率的な現象。

システムノイズ 動きが思う様にいかないこと は、いつどのくらいになるか は確率的な現象。 観測ノイズ 観測した時に含まれている誤 差もいつどのくらいになるか は確率的な現象。

速度 𝑣 𝑚/𝑠 距離測定 𝐿! 状態⽅程式 位置 𝑥! 観測⽅程式 出発点の壁 𝑥! = 𝑥!"# + 𝑣 𝐿! = 𝑥! 時間 𝑡! = 𝑡!"# + 1 時間は1秒ごとに増えていくとします

速度 𝑣 𝑚/𝑠 距離測定 𝐿! 状態⽅程式 位置 𝑥! 観測⽅程式 出発点の壁 𝑥! = 𝑥!"# + 𝑣 𝐿! = 𝑥! 時間 ノイズがなければ観測⽅程式からセン サーで測った距離がロボットの位置を表 していることになるが、実際はノイズが あるのでそうはならない。 𝑡! = 𝑡!"# + 1 時間は1秒ごとに増えていくとします

速度 𝑣 𝑚/𝑠 距離測定 𝐿! 状態⽅程式 位置 𝑥! 観測⽅程式 出発点の壁 ノイズがなければ観測⽅程式からセンサ で測った距離がロボットの位置を表して いることになるが、実際はノイズがある のでそうはならない。 𝑥! = 𝑥!"# + 𝑣 + 𝑤$ 𝐿! = 𝑥! + 𝑤% 𝑤! :システムノイズ 𝑤" :観測ノイズ

速度 𝑣 𝑚/𝑠 距離測定 𝐿! 状態⽅程式 位置 𝑥! 観測⽅程式 出発点の壁 ノイズがなければ観測⽅程式からセンサ で測った距離がロボットの位置を表して いることになるが、実際はノイズがある のでそうはならない。 𝑥! = 𝑥!"# + 𝑣 + 𝑤$ 𝐿! = 𝑥! + 𝑤% 𝑤! :システムノイズ 𝑤" :観測ノイズ 実際に得られるデータは 観測（距離測定）𝐿# のみ

システムノイズがなけ ればこの位置にロボッ トがいたはず（理想） 状態⽅程式 𝑥! = 𝑥!"# + 𝑣 + 𝑤$ 観測⽅程式 𝐿! = 𝑥! + 𝑤% 𝑤! 𝑤" ロボットの位置 :システムノイズ :観測ノイズ

状態⽅程式 𝑥! = 𝑥!"# + 𝑣 + 𝑤$ 観測⽅程式 𝐿! = 𝑥! + 𝑤% システムノイズがなけ ればこの位置にロボッ トがいたはず（理想） ロボットの位置 𝑤! 𝑤" :システムノイズ :観測ノイズ

理想からの変動を取り出して ノイズの影響を強調した 状態⽅程式 𝑥)! = 𝑥)!"# + 𝑤$ 観測⽅程式 𝐿* ! = 𝑥)! + 𝑤% 𝑤! :システムノイズ 𝑤":観測ノイズ ここで ロボットの位置の変化分 上５０秒間 下５００秒間 𝑥)! = 𝑥! − 𝑛𝑣 𝐿* ! = 𝐿! − 𝑛𝑣

理想からの変動を取り出して ノイズの影響を強調した 状態⽅程式 𝑥)! = 𝑥)!"# + 𝑤$ 実際にセンサから得られる情報はオレ ンジの情報なので、そこから⻘⾊のロ ボットの位置情報を推定する。 カルマンフィルタ ロボットの位置の変化分 上５０秒間 下５００秒間 観測⽅程式 𝐿* ! = 𝑥)! + 𝑤% 𝑤! :システムノイズ 𝑤":観測ノイズ ここで 𝑥)! = 𝑥! − 𝑛𝑣 𝐿* ! = 𝐿! − 𝑛𝑣

ノイズの性質 ノイズの値はランダムに現れるも のとみなします。 そしてたくさんのノイズの値を集 められたとすると、その平均は０と 考えます。 平均が０で同じになっても、値の ばらつき⽅には違いが出てきます。 ばらつき⽅を⽰す値を分散σ２と ⾔います。その平⽅根が標準偏差σ です。 ノイズを１０万回シミュレーション で⽣成して、それぞれの値の回数を 数えてヒストグラムを作りました。 平均０の ノイズ σ＝１ σ＝３ ノイズのヒストグラム 分散の⼤きさによって釣鐘型 の分布の裾野が広がる

線形化 話を簡単にしたいので、理想からの変動だけを⾒ていきます こちらで話を進めます 状態⽅程式 状態⽅程式 観測⽅程式 観測⽅程式 𝑥# = 𝑥#$% + 𝑣 + 𝑤! 𝐿# = 𝑥# + 𝑤" 𝑥'# = 𝑥'#$% + 𝑤! 𝐿( # = 𝑥'# + 𝑤" 右の２式で戻せる 𝑥'# = 𝑥# − 𝑛𝑣 𝐿( # = 𝐿# − 𝑛𝑣

カルマンフィルタの考え⽅ 1. ⾃分（ロボット）はどこにい るのか？システムノイズは０ （最も確率が⾼いノイズの 値）で状態⽅程式で予測する 2. センサからの観測情報を使っ て、予想を補正する。 3. 補正の際、推定のばらつきが ⼩さいと考えれば予想を重視 し、観測ノイズが⼩さいと考 えれば観測をより重視する

カルマンフィルタの考え⽅ 1. ⾃分（ロボット）はどこにい るのか？システムノイズは０ （最も確率が⾼いノイズの 値）で状態⽅程式で予測する 2. センサからの観測情報を使っ て、予測を補正する。 3. 補正の際、推定のばらつきが ⼩さいと考えれば予想を重視 し、観測ノイズが⼩さいと考 えれば観測をより重視する 𝑥̅# = 𝑥$#$% 最新の予測 ⼀つ前の推定

カルマンフィルタの考え⽅ 1. ⾃分（ロボット）はどこにい るのか？システムノイズは０ （最も確率が⾼いノイズの 値）で状態⽅程式で予測する 2. センサからの観測情報を使っ て、予測を補正する。 3. 補正の際、推定のばらつきが ⼩さいと考えれば予想を重視 し、観測ノイズが⼩さいと考 えれば観測をより重視する 𝑥̅# = 𝑥$#$% 最新の予測 ⼀つ前の推定 𝑥$# = 𝑥̅# + 𝐾(𝐿( # − 𝑥̅# ) イノベーション カルマンゲイン

カルマンフィルタの考え⽅ 𝑥̅# = 𝑥$#$% 1. ⾃分（ロボット）はどこにい るのか？システムノイズは０ （最も確率が⾼いノイズの 値）で状態⽅程式で予測する 最新の予測 2. センサからの観測情報を使っ て、予測を補正する。 最新の推定 3. 補正の際、推定のばらつきが ⼩さいと考えれば予想を重視 し、観測ノイズが⼩さいと考 えれば観測をより重視する ⼀つ前の推定 カルマンゲイン 𝑥$# = 𝑥̅# + 𝐾# (𝐿( # − 𝑥̅# ) イノベーション カルマンゲインに関しては以下の式から計算して 使います。 𝐾! = 𝜎(" ! # 𝜎(" ! # + 𝜎$ # 𝜎!! " # ：位置のばらつきの予測値(あとで説明) 𝜎$ # ：観測ノイズの分散

位置推定式の意味についての補⾜ 𝑥-! = 𝑥̅! + 𝐾! (𝐿* ! − 𝑥̅! ) 𝐾# = 𝜎)! # （１） $ $ 𝜎)! # + 𝜎" $ （２） （１）式に（２）式を代⼊して整理すると次式になる 𝑥-! = 𝜎% & & 𝜎3$ ! + 𝜎% 𝑥 ̅ + ! & 観測が信⽤できるということは、ば らつき𝜎! " が⼩さいので推定値に対し てこの項の影響（予測の影響）は⼩ さくなる 𝜎3$ ! & & 𝜎3$ ! + 𝜎% *! 𝐿 &

位置推定のばらつきの予測と推定 カルマンフィルタは位置の推定と同時に位置推定のばらつき（分 散）も予測して推定します。 位置推定のばらつき（確からしさ）は最初わからない事が多いの ですが、適当な値から始めてもこの推定によって確からしい値に 段々収束し、通常ばらつきが⼩さくなり位置推定精度が上がってい きます。 状態⽅程式からこ の様になります 位置の推定のばらつきの（分散）の予測 $ $ 𝜎)! # = 𝜎,! #%& + 𝜎! $ 位置の推定のばらつきの（分散）の推定 $ 𝜎,! # = 1 − 𝐾# 𝜎)! # $ 𝜎! # ：システムノイズの分散 カルマンゲインを 使って予測から推 定へ更新してます

カルマンフィルタ位置推定のながれ 1. 2. 3. 4. 5. 6. 位置を予測 位置の分散を予測 カルマンゲインを計算 カルマンゲインと位置の予測とイノベーションから位置を推定 カルマンゲインと位置の分散の予測から位置の分散を推定 １にもどり繰り返す ※初期の位置と分散、システムノイズの分散、観測の分散は あらかじめ与えます

課題 配布したエクセルファイルには上の図で⽰される500秒間のロボットの位置の観測 データが含まれている。 このデータからカルマンフィルタを⽤いて、ロボットの位置を推定しなさい。 ロボットの初期位置 ０ 位置推定の初期分散 １００ 観測の分散 ９ システムノイズの分散 １

計算は配布したエクセルシートを⽤いて⾏いなさい。提出はファイル名を 「番号⽒名カルマンフィルタ」にして提出しなさい。 推定した結果は、下図に⽰す様に、観測データと重ねてグラフにして⽰しなさい