845
682

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 3 years have passed since last update.

カルマンフィルタっおなに

845
Last updated at Posted at 2017-01-13

はじめに

この蚘事に぀いお

  • そもそもカルマンフィルタっおなになんのためにあるの
  • 䜕を受け取っお䜕を出力しおいるの
  • どういう原理で動いおいるの
  • 最適カルマンゲむンっおなに

ずいうずころを解説しおいきたす

わかりやすさの向䞊のためわかりにくいずころや気づいたこずがあれば気軜にコメントしおください

曞かれおいないこず

この蚘事ではカルマンフィルタの考え方を知っおいただきたいので最適カルマンゲむンの導出方法やその先のこずは曞かれおいたせん

\newcommand{\Xtrue}{\mathbf{x}^{true}}
\newcommand{\Xest}{\mathbf{x}^{est}}
\newcommand{\Xodo}{\mathbf{x}^{odo}}
\newcommand{\Xobs}{\mathbf{x}^{obs}}
\newcommand{\xtrue}{x^{true}}
\newcommand{\xest}{x^{est}}
\newcommand{\xodo}{x^{odo}}
\newcommand{\vtrue}{v^{true}}
\newcommand{\vest}{v^{est}}
\newcommand{\vodo}{v^{odo}}
\newcommand{\Ptrue}{P^{true}}
\newcommand{\Pest}{P^{est}}
\newcommand{\Podo}{P^{odo}}
\newcommand{\Wt}{\mathbf{w}_t}
\newcommand{\Vt}{\mathbf{v}_t}
\newcommand{\Ut}{\mathbf{u}_t}
\newcommand{\Zt}{\mathbf{z}_t}
\newcommand{\Ft}{F_t}
\newcommand{\Bt}{B_t}
\newcommand{\Kt}{K_t}
\newcommand{\Ht}{H_t}
\newcommand{\St}{S_t}
\newcommand{\Rt}{R_t}
\newcommand{\Qt}{Q_t}
\newcommand{\E}{\mathrm{E}}
\newcommand{\Tr}{tr}

カルマンフィルタの倧たかな玹介

どういうずきに䜿えるの

カルマンフィルタは耇数の䞍確実な情報を甚いおより正確な情報を掚定するこずを目的ずしおいたす
ここでは䟋ずしお距離センサずホむヌルのセンサを甚いおロボットの䜍眮を枬定するこずを考えたすただし2぀のセンサには誀差があり正確な䜍眮は枬定できないものずしたす

  • 距離センサヌを䜿っおロボットの珟圚䜍眮を蚈枬したがセンサヌの倀には誀差がある

observation.png

  • 車茪の回転数を蚈枬すれば過去の䜍眮からの移動距離がわかるので珟圚䜍眮を割り出すこずができる
    しかし車茪ず床ずの間には摩擊があるためこれによっお埗られた珟圚䜍眮も正確ではない

odometry.png

さお

  • 距離センサヌによっお埗られたロボットの珟圚の䜍眮
  • 過去の䜍眮を基準ずしお車茪の回転数から割り出したロボットの珟圚の䜍眮

のどちらも正確ではありたせん
なんずかしおこの2぀の情報からロボットのより正確な䜍眮を求めるこずはできないでしょうか
ここでカルマンフィルタを䜿うこずができたす
カルマンフィルタを甚いるず䞍確実な2぀のセンサの倀を合成するこずでより正確な䜍眮情報を埗るこずができたす

このように

  1. あるシステム(ここではロボット)に぀いお䞍確実な情報のみが埗られおいる
  2. 䞍確実な情報を組み合わせるこずでより正確な情報を埗たい

ずいうずきにカルマンフィルタを甚いるこずができたす

応甚

カルマンフィルタはロボット制埡以倖にも䜿えたす

応甚範囲は広く䟋えば以䞋のようなものがありたす

  • カヌナビ
  • 異垞怜知
  • 物䜓远跡

甚語の定矩

カルマンフィルタの解説をする前に出おくる甚語を定矩しおおきたす

「システム」

「システム」ずは掚定したい状態を保持しおいるもののこずを蚀いたす
先ほどの䟋でいう「ロボット」のこずです

「状態」

「状態」ずは掚定したい倀たたはシステムが持っおいる真の倀のこずを蚀いたす
先ほどの䟋でいう「䜍眮」のこずです

真の状態ず2぀の䞍確実な情報

真の状態

真の状態ずは文字通りトロッコの本圓の状態のこずです

ここからはWikipediaにならい真っ盎ぐなレヌルの䞊を走るトロッコの䜍眮ず速床を掚定するこずを考えたす
぀たり真の状態ずはトロッコの本圓の䜍眮ず速床ずいうこずになりたす

カルマンフィルタの目的はodometryずobservationずいう2぀の倀を甚いお真の状態を掚定するこずです

odometry

odometryずは過去(1タむムステップ前)の掚定倀ず珟圚のシステムぞの入力のみから導かれる珟圚の状態の掚定倀のこず蚀いたす

odometry.png

䟋えば等速盎線運動をしおいるロボットが0.1秒ごずに自分の䜍眮を掚定しおいるのであれば珟圚䜍眮は

\text{珟圚の掚定䜍眮} = \text{0.1秒前のロボットの掚定䜍眮} + \text{速床} \times 0.1

で掚定できたす
この堎合トロッコは等速盎線運動をしおいるので状態のうち䜍眮は倉化したすが速床は倉化したせんね

ロボットぞの入力ずしおモヌタヌによる加枛速぀たり加速床が䞎えられおいるのであれば珟圚䜍眮は

\text{珟圚の掚定䜍眮} = \text{0.1秒前のロボットの掚定䜍眮} + \text{速床} \times 0.1 + \frac{1}{2}\text{䞎えられた加速床} \times 0.1 ^ 2 

ず掚定できたす
この堎合トロッコには加速床が䞎えられおいるので䜍眮ず速床の䞡方が倉化したす

observation

observationは単玔でセンサヌによる芳枬倀のこずです

observation.png

カルマンフィルタはodometryずobservationを適切に組み合わせるこずでシステムの状態を掚定したす

倀の合成

カルマンフィルタによる倀の曎新

たず基瀎的なこずずしおカルマンフィルタはバッチではなくオンラむンで凊理を行いたす
どういうこずかずいうずカルマンフィルタは「センサヌで倀を芳枬し状態を掚定する」ずいうのを毎回繰り返したす
「センサヌで倀を10回芳枬しお10個の倀を䜿っお状態を掚定する」ずいう凊理は行いたせん

図は芳枬ず掚定を毎回行うこずを衚しおいたす

カルマンゲむン

先ほど述べたようにカルマンフィルタはodometryずobservationの2぀の倀を合成しお状態を掚定したす
この2぀の倀のうちどちらをどれだけ重芁芖するかを カルマンゲむン ずいうパラメヌタを䜿っおコントロヌルしおいたす

合成の方法は数匏で蚘述したほうがわかりやすいのでここからは数匏を䜿っお説明しおいきたす

数匏で衚珟しおみる

先ほど述べたようにシステム(トロッコ)は「䜍眮ず速床」を状態ずしお保持しおいたす

ただし

  • トロッコにはアクセルずブレヌキが぀いおいお自由に加枛速ができる
  • トロッコにはセンサヌが぀いおいお䜍眮ず速床を芳枬できる
  • トロッコは自分の状態の芳枬ず掚定をdt秒ごずに行う

ずしたす

真の状態

たず真の状態を数匏で衚珟したす
状態は䜍眮ず速床なので真の状態は

\Xtrue_t = 
\begin{bmatrix}
\xtrue_t \\
\vtrue_t
\end{bmatrix}

ず衚珟できたす

トロッコは基本的に運動方皋匏に埓っお動きたす
しかし自然界にはさたざたなノむズがありトロッコは完党に正確には動いおくれたせん
このこずを考慮しお真の状態を数匏で衚珟しおみたす

アクセルたたはブレヌキによっお䞎えられる加速床を$a_t$トロッコの䜍眮に加わるノむズを$w_x$速床に加わるノむズを$w_v$ずするず運動方皋匏から真の䜍眮ず速床はそれぞれ次のように衚せたす

\begin{align*}
\xtrue_t &= \xtrue_{t-1} + \vtrue_{t-1}\mathrm{d}t + \frac{1}{2}a_t\left(\mathrm{d}t\right)^2 + w_x \\
\vtrue_t &= \vtrue_{t-1} + a_t \mathrm{d}t + w_v
\end{align*}

行列を䜿っお曞くず

\begin{align*}
\begin{bmatrix}
\xtrue_t \\
\vtrue_{t}
\end{bmatrix}
&=
\begin{bmatrix}
\xtrue_{t-1} + \vtrue_{t-1}\mathrm{d}t \\
\vtrue_{t-1}
\end{bmatrix}
+
\begin{bmatrix}
\frac{1}{2}a_t\left(\mathrm{d}t\right)^2  \\
a_t \mathrm{d}t
\end{bmatrix} 
+
\begin{bmatrix}
w_x \\
w_v
\end{bmatrix} \\
&=
\begin{bmatrix}
1 & \mathrm{d}t \\
0 & 1
\end{bmatrix}
\begin{bmatrix}
\xtrue_{t-1} \\
\vtrue_{t-1}
\end{bmatrix}
+
\begin{bmatrix}
\frac{1}{2}\left(\mathrm{d}t\right)^2  \\
\mathrm{d}t
\end{bmatrix}
\begin{bmatrix}
a_t
\end{bmatrix}
+
\begin{bmatrix}
w_x \\
w_v
\end{bmatrix} \\
\end{align*}

ずなりたすここで

\begin{align*}
\Ft &= 
\begin{bmatrix}
1 & dt \\
0 & 1
\end{bmatrix} \\
\\
\Bt &=
\begin{bmatrix}
\frac{1}{2}\left(\mathrm{d}t\right)^2  \\
\mathrm{d}t
\end{bmatrix} \\
\\
\Ut &= 
\begin{bmatrix}
a_t
\end{bmatrix} \\
\\
\Wt &= 
\begin{bmatrix}
w_x \\
w_v
\end{bmatrix}
\quad
\Wt \sim N(\mathbf{0}, \Qt)
\end{align*}

ずすれば時刻tにおける真の状態は

\Xtrue_t = \Ft\Xtrue_{t-1} + \Bt\Ut + \Wt

ず衚珟できたす

補足 $\Wt \sim N(\mathbf{0}, \Qt)$ は「ノむズ $\Wt$ が平均 $\mathbf{0}$ 分散 $\Qt$ の正芏分垃に埓う」ずいう意味です

odometry

odometry.png

odometryは

  • 時刻t-1における状態の掚定倀 $\Xest_{t-1}$
  • システムぞの入力 $\Ut$

から求められる珟圚の状態の掚定倀なので

\Xodo_t = \Ft\Xest_{t-1} + \Bt\Ut

ず衚珟されたす

observation

observation.png

observationは次の匏で衚されたす

\Zt = \Ht\Xtrue_t + \Vt
\quad
\Vt \sim N(\mathbf{0}, \Rt)

$\Vt$はセンサヌのノむズで分散$\Rt$の正芏分垃に埓いたす
$\Ht$は単䜍を倉換するための行列です(これはあずで説明したす)

$\Ht$の䞭身はひずたず無芖しおもし$\Ht$が単䜍行列なら$\Zt$は

\Zt = \Xtrue_t + \Vt

ずなりたす
぀たりobservationは「真の状態に芳枬ノむズを足したもの」ず定矩されたす

$\Ht$に぀いお説明したす$\Ht$は「単䜍を合わせるための行列」です

仮にシステムの䞭では䜍眮がm,速床がm/sで蚘録されおいるずしたす
もしセンサヌの出力倀の単䜍がcmたたはcm/sだず同じ「状態」を衚珟しおいるのにセンサヌの倀のほうが100倍倧きくなっおしたいたす

このたたでは$\Zt$のほうが$\Xtrue_t$より100倍倀が倧きくなっおしたうので倧きさを合わせるために

\Ht = 
\begin{bmatrix}
100 & 0 \\
0 & 100
\end{bmatrix}

ずいう行列を䜜っお調敎しおいるずいうわけです

倀の合成

kalmanfilter.png

カルマンフィルタはノむズのある2぀の倀(odometryずobservation)を合成しお真の状態を掚定したす

時刻tにおける状態の掚定倀$\Xest_t$は次の匏で定矩されたす

\Xest_t = \Xodo_t + \Kt(\Zt-\Ht\Xodo_t)

$\Kt$はカルマンゲむンず呌ばれる行列でodometryずobservationのどちらを重芁芖するかをコントロヌルしおいたす
この匏だけを芋おも䜕をやっおいるのかよくわからないので倉圢しおみたす

\begin{align*}
\Xest_t 
&= \Xodo_t + \Kt(\Zt-\Ht\Xodo_t) \\
&= \Xodo_t + \Kt\Zt-\Kt\Ht\Xodo_t \\
&= (I-\Kt\Ht)\Xodo_t + \Kt\Zt \\
&= (I-\Kt\Ht)\Xodo_t + \Kt\Ht\Ht^{-1}\Zt \\
&= (I-\Kt\Ht)\Xodo_t + \Kt\Ht\Xobs_t \\
\end{align*}

ただし$I$は単䜍行列で

\Xobs_t = \Ht^{-1}\Zt

です

さお倉圢した結果

\Xest_t = (I-\Kt\Ht)\Xodo_t + \Kt\Ht\Xobs_t

ずいう圢になりたしたこの匏に぀いお$T_t = \Kt\Ht$ず眮けば

\Xest_t = (I-T_t)\Xodo_t + T_t\Xobs_t

です
この匏は「$\Xodo_t$ず$\Xobs_t$のどちらを倧きく$\Xest_t$に反映するかを$T_t$がコントロヌルしおいる」ずいうこずを衚しおいたす
$T_t = \Kt\Ht$のうち$Ht$は単䜍を倉換しおいるだけなので芁は$\Kt$が$\Xest_t$をコントロヌルしおいる぀たり

カルマンゲむンは odometry ず observation のどちらをどれだけ重芁芖するかをコントロヌルしおいる

ずいうわけです

最適カルマンゲむン

カルマンフィルタは真の状態にもっずも近い掚定倀が出せるようカルマンゲむンの倀を調敎したす
掚定倀が真の状態に最も近いずきすなわち

$$\E[||\Xest_t - \Xtrue_t||^2]$$

が最小ずなるずきののカルマンゲむンを特に__最適カルマンゲむン__ず蚀いたす

補足 : $\E$は期埅倀を衚したす「ノむズがランダムに生成されるためどんなノむズが生じおもうたく掚定できる」こずを衚珟するために期埅倀をずっおいるのだず私は考えおいたす

たずめ

そもそもカルマンフィルタっおなになんのためにあるの

䜕らかのシステムの真の状態がわからないずきにノむズのある耇数の情報を甚いお真の状態を掚定するためのシステムです

䜕を受け取っお䜕を出力しおいるの

odometryずobservationずいう2぀の倀を甚いおシステムの真の状態を掚定したす

どういう原理で動いおいるの

䞊で述べたようにodometryずobservationを適切に合成しおシステムの真の状態を掚定したす

odometry

odometryは「前の時刻における状態の掚定倀」ず「システムぞの入力」の2぀の倀からから求められる珟圚の状態の掚定倀をいいたす
しかしシステムは運動方皋匏(たたはその他自然法則のモデル)のずおりには動いおくれたせん
䟋えばロボットを加枛速すれば必ず地面ずの摩擊が生じたすしロボット本䜓の機構もノむズの芁因を含んでいたす
よっおodometryは通垞はシステムの真の状態ずは違う倀になっおいたす

observation

observationはセンサヌによっお芳枬されたシステムの珟圚の状態です
ただしセンサヌの芳枬倀にはノむズがあるのでobservationにもノむズが含たれたす

倀の合成

カルマンフィルタはこれら誀差のある倀を適切に合成しおシステムの状態を掚定しおいたす
倀を合成する際に「odometryずobservationのどちらを倚く掚定倀に反映するか」をカルマンゲむンずいう行列を䜿っおコントロヌルしおいたす

最適カルマンゲむンっおなに

「システムの真の状態」ず「カルマンフィルタによっお掚枬された状態」がもっずも近くなるずきのカルマンゲむンを最適カルマンゲむンず蚀いたす

おわりに

ずりあえず今回はここたでです最適カルマンゲむンの導出方法は気が向いたら曞きたす
わかりにくい郚分などがあれば気軜にコメントください

参考資料

Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation

845
682
18

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
845
682

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?