June 19, 2011

Pythonでカルマンフィルタを実装してみる

カルマンフィルタは、時間変化するシステムの、誤差のある離散的な観測から現在の状態を推定する手法。Wikipediaの記事(カルマンフィルター)がわかりやすい。

状態方程式と観測方程式が次のように与えられているとき
{\bf x}_{t+1}={\bf Ax}_{t}+{\bf Bu}_{t}+{\bf w}_{t+1} (状態方程式)
{\bf y}_{t}={\bf Cx}_{t}+{\bf v}_{t} (観測方程式)
{\bf w}_{t}\sim N(0,{\bf Q}){\bf v}_{t}\sim N(0,{\bf R}) (ノイズ)
p({\bf x}_{t}|{\bf y}_{0:t})=N({\bf x}_{t};{\bf \mu}_{t},{\bf \Sigma}_{t}) (フィルタ分布)
線形カルマンフィルタ(LKF; Linear Kalman Filter)は μt, Σt, ut, yt+1 を入力として、 μt+1, Σt+1を出力する。1ステップのプロセスは以下のとおり。

# prediction
{\bf \hat{\mu}}_{t+1} \leftarrow {\bf A\mu}_{t}+{\bf Bu} (現在の推定値)
{\bf \hat{\Sigma}}_{t+1} \leftarrow {\bf Q}+{\bf A\Sigma}_t {\bf A}^T (現在の誤差行列)
# update
{\bf \tilde{y}}_{t+1} \leftarrow {\bf y}_{t+1}-{\bf C\hat{\mu}}_{t+1} (観測残差)
{\bf S}_{t+1} \leftarrow {\bf C}{\bf \hat{\Sigma}}_{t+1}{\bf C}^T+{\bf R} (観測残差の共分散)
{\bf K}_{t+1} \leftarrow {\bf \hat{\Sigma}}_{t+1}{\bf C}^T{\bf S}^{-1} (最適カルマンゲイン)
{\bf \mu}_{t+1} \leftarrow {\bf \hat{\mu}}_{t+1}+{\bf K}_{t+1}{\bf \tilde{y}}_{t+1} (更新された現在の推定値)
{\bf \Sigma}_{t+1} \leftarrow {\bf \hat{\Sigma}}_{t+1}-{\bf K}_{t+1}{\bf C}{\bf \hat{\Sigma}}_{t+1} (更新された現在の誤差行列)
観測を得るごとにPredictionとUpdateを繰り返すことで、現在の状態を推定します。

導出は後述(予定)。

例題を。
2次元座標において、あるロボットがt=0に原点を出発して、速度(2,2)で動くとする。ロボットの進路は風などの影響を受け(σxy=1)、毎秒ごとに観測できるGPSによる位置座標には計測誤差(σxy=2)があるとする。このとき、観測された軌跡から実際の軌跡を推定する。
係数は
{\bf A}=\left[ \begin{array}{cc} 1 & 0 \\ 0 & 1 \\ \end{array} \right]{\bf B}=\left[ \begin{array}{cc} 1 & 0 \\ 0 & 1 \\ \end{array} \right]{\bf u}_{const}=\left[ \begin{array}{c} 2 \\ 2 \\ \end{array} \right]{\bf Q}=\left[ \begin{array}{cc} 1 & 0 \\ 0 & 1 \\ \end{array} \right]
{\bf C}=\left[ \begin{array}{cc} 1 & 0 \\ 0 & 1 \\ \end{array} \right]{\bf R}=\left[ \begin{array}{cc} 2 & 0 \\ 0 & 2 \\ \end{array} \right]
初期値は
{\bf \mu}_0=\left[ \begin{array}{c} 0 \\ 0 \\ \end{array} \right]{\bf \Sigma}_0=\left[ \begin{array}{cc} 0 & 0 \\ 0 & 0 \\ \end{array} \right]

コードは以下。要SciPy。(やってる事自体はSciPyは必須ではないのだけど。)



実行結果は下図。


Fig.1 が実際の軌跡。が観測値。が推定値。

観測に対してカルマンフィルタによる推定値が、実際の軌跡に近いことがわかります。