satomacoto: 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, u_t, y_t+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)で動くとする。ロボットの進路は風などの影響を受け（σ_x=σ_y=1）、毎秒ごとに観測できるGPSによる位置座標には計測誤差（σ_x=σ_y=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]$