はじめに
この記事について
ここではカルマンフィルタの「考え方」を解説します．
「カルマンフィルタを使ってみたいけど、いきなり無限インパルス応答とか隠れマルコフモデルとか言われてもわかんない」という方を対象としています．
この記事では
- そもそもカルマンフィルタってなに？なんのためにあるの？
- 何を受け取って、何を出力しているの？
- どういう原理で動いているの？
- 最適カルマンゲインってなに？
というところを解説していきます．
書かれていないこと
この記事ではカルマンフィルタの考え方を知っていただきたいので、最適カルマンゲインの導出方法やその先のことは書かれていません．要望があれば書くので、続きが見たいという方がいらっしゃいましたらぜひコメントください．
\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つの情報を合成することでより正確な位置情報を得ることができます．
このように，
- あるシステム(ここではロボット)について不確実な情報のみが得られている
- 不確実な情報を組み合わせることで，より正確な情報を得たい
というときにカルマンフィルタを用いることができます．
応用
カルマンフィルタはロボット制御以外にも使えます．
応用範囲は広く、例えば以下のようなものがあります。
- カーナビ
- 異常検知
- 物体追跡
用語の定義
カルマンフィルタの解説をする前に，出てくる用語を定義しておきます．
「システム」
「システム」とは，推定したい「状態」を保持しているもののことを言います．
先ほどの例でいう「ロボット」のことです．
「状態」
「状態」とは，推定したい値，またはシステムが持っている真の値のことを言います．
先ほどの例でいう「位置」のことです．
カルマンフィルタによる値の更新
カルマンフィルタはバッチではなくオンラインで処理を行います．
どういうことかというと，カルマンフィルタは「センサーで値を観測し状態を推定する」というのを毎回繰り返します．
「センサーで値を10回観測して，10個の値を使って状態を推定する」という処理は行いません．
図は観測と推定を毎回行うことを表しています．
真の状態
真の状態とは，文字通りトロッコの本当の位置と速度のことです．
カルマンフィルタの目的は，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は単純で，センサーによる観測値のことです．
カルマンフィルタは，これら2つの値を組み合わせることで，システムの状態を推定します．
数式で表現してみる
ここからはWikipediaにならい，真っ直ぐなレールの上を走るトロッコの位置と速度を推定することを考えます．すなわちシステムの状態は「位置と速度」となります．
ただし，
- トロッコにはアクセルとブレーキがついていて自由に加減速ができる
- トロッコにはセンサーがついていて位置と速度を観測できる
- トロッコは自分の状態の観測と推定を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-1} &= \vtrue_{t-1} + a_t \mathrm{d}t + w_v
\end{align*}
行列を使って書くと，
\begin{align*}
\begin{bmatrix}
\xtrue_t \\
\vtrue_{t-1}
\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} \\
\end{align*}
\begin{align*}
\Wt =
\begin{bmatrix}
w_x \\
w_v
\end{bmatrix}
\quad
\Wt \sim N(0, \Qt)
\end{align*}
とすれば，時刻tにおける真の状態は
\Xtrue_t = \Ft\Xtrue_{t-1} + \Bt\Ut + \Wt
と表現できます．
補足： $\Wt \sim N(0, \Qt)$はノイズ$\Wt$が平均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(0, \Rt)
$\Vt$はセンサーのノイズで，分散$\Rt$の正規分布に従います．
$\Ht$は単位を変換するための行列です(これはあとで説明します)．
$\Ht$の中身はひとまず無視して，もし$\Ht$が単位行列なら，$\Zt$は
\Zt = \Xtrue_t + \Vt
となります．
つまりobservationは，「真の状態に観測ノイズを足したもの」と定義されます．
$\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 のどちらをどれだけ重要視するかをコントロールしている
というわけです．
カルマンフィルタは、真の状態にもっとも近い推定値が出せるようなカルマンゲインの値を調整します．
なお，推定値が真の状態に最も近いときのカルマンゲインを特に最適カルマンゲインと言います．
おわりに
とりあえず今回はここまでです．最適カルマンゲインの導出方法は気が向いたら書きます．
書いてほしいことがあれば気軽にコメントください．