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

カルマンフィルタは、時間変化するシステムの、誤差のある離散的な観測から現在の状態を推定する手法。Wikipediaの記事（カルマンフィルター）がわかりやすい。 状態方程式と観測方程式が次のように与えられているとき （状態方程式） （観測方程式） （ノイズ） （フィルタ分布）線形カルマンフィルタ(LKF; Linear Kalman Filter)は μt, Σt, ut, yt+1 を入力として、 μt+1, Σt+1を出力する。1ステップのプロセスは以下のとおり。 # prediction （現在の推定値） （現在の誤差行列）# update （観測残差） （観測残差の共分散） （最適カルマンゲイン） （更新された現在の推定値） （更新された現在の誤差行列）観測を得るごとにPredictionとUpdateを繰り返すことで、現在の状態を推定します。 導出は後述（予定）。 例題を。 2次元