はじめに

この記事について

そもそもカルマンフィルタってなに？なんのためにあるの？
何を受け取って，何を出力しているの？
どういう原理で動いているの？
最適カルマンゲインってなに？

というところを解説していきます．

わかりやすさの向上のため，わかりにくいところや気づいたことがあれば気軽にコメントしてください．

書かれていないこと

この記事ではカルマンフィルタの考え方を知っていただきたいので，最適カルマンゲインの導出方法やその先のことは書かれていません．

\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つのセンサには誤差があり，正確な位置は測定できないものとします．

距離センサーを使ってロボットの現在位置を計測したが，センサーの値には誤差がある

車輪の回転数を計測すれば過去の位置からの移動距離がわかるので，現在位置を割り出すことができる．
しかし車輪と床との間には摩擦があるため，これによって得られた現在位置も正確ではない

さて，

距離センサーによって得られたロボットの現在の位置
過去の位置を基準として車輪の回転数から割り出したロボットの現在の位置

のどちらも正確ではありません．
なんとかしてこの2つの情報から，ロボットのより正確な位置を求めることはできないでしょうか．
ここでカルマンフィルタを使うことができます．
カルマンフィルタを用いると，不確実な2つのセンサの値を合成することでより正確な位置情報を得ることができます．

このように，

あるシステム(ここではロボット)について不確実な情報のみが得られている
不確実な情報を組み合わせることで，より正確な情報を得たい

というときにカルマンフィルタを用いることができます．

応用

カルマンフィルタはロボット制御以外にも使えます．

応用範囲は広く，例えば以下のようなものがあります．

カーナビ
異常検知
物体追跡

用語の定義

カルマンフィルタの解説をする前に，出てくる用語を定義しておきます．

「システム」

「システム」とは，推定したい状態を保持しているもののことを言います．
先ほどの例でいう「ロボット」のことです．

「状態」

「状態」とは，推定したい値，またはシステムが持っている真の値のことを言います．
先ほどの例でいう「位置」のことです．

真の状態と2つの不確実な情報

真の状態

真の状態とは文字通りトロッコの本当の状態のことです．

ここからはWikipediaにならい，真っ直ぐなレールの上を走るトロッコの位置と速度を推定することを考えます．
つまり真の状態とはトロッコの本当の位置と速度ということになります．

カルマンフィルタの目的は，odometryとobservationという2つの値を用いて真の状態を推定することです．

odometry

odometryとは，過去(1タイムステップ前)の推定値と，現在のシステムへの入力のみから導かれる，現在の状態の推定値のこと言います．

例えば等速直線運動をしているロボットが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は単純で，センサーによる観測値のことです．

カルマンフィルタは，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は

時刻t-1における状態の推定値 $\Xest_{t-1}$
システムへの入力 $\Ut$

から求められる現在の状態の推定値なので，

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

と表現されます．

observation

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}

という行列を作って調整しているというわけです．

値の合成

カルマンフィルタは，ノイズのある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