概要

線形カルマンフィルタをPythonのnumpyパッケージを用いて実装します。

参照したプログラムは、Artificial Intelligence for RoboticのLesson 5.6で課題として提出されているもので、mathパッケージを使用していました。
有名な課題なので、類似したプログラムは、ネットで検索すれば幾らでも出てきます。本記事とほぼ同じもの多く、本記事の解説は屋上屋を重ねているだけとも言えます。

Artificial Intelligence for Roboticがどのようなものかについては、以下をご参照ください。

カルマンフィルタを避けてきた社会人プログラマが自動運転の勉強を始める際の最短経路

実装

以下は、2次元を移動する物体の位置と速度を追跡するプログラムです。
状態は2次元座標と2次元速度を並べた$(x, y, \dot x, \dot y)$の4次元状態です。

計測は位置だけを取得でき、速度は計測不能とします。ここでは0.1秒間隔で6回計測したとします。

$P$は共分散行列で、この値が大きいほど予測が広く分布し、値の精度が悪いことになります。
$F$は状態遷移行列です。次の時刻での状態を求めるのに使用します。
$H$は観測行列です。観測値は位置だけなので、4次元状態から位置だけを抽出する役割を持ちます。
$R$はノイズです。計測結果はノイズにより不確かなものとなります。

カルマンフィルタは予測の不確かさと、計測の不確かさを共に減らし、より精度の良い予測値を導出することができます。
（ただし、4次元状態、遷移確率、計測の3つがガウス分布に従うという条件がつきます。）

以下のプログラムでは6回の計測後の、位置と座標の予測値を出力します。

KalmanFilter.py

import numpy as np

measurements = [[7., 15.], [8., 14.], [9., 13.], [10., 12.], [11., 11.], [12., 10.]] #位置[x,y]の計測結果
initial_xy = [6., 17.] #初期位置

dt = 0.1 #計測間隔

x = np.array([[initial_xy[0]], [initial_xy[1]], [0.], [0.]]) # 初期位置と初期速度を代入した「4次元状態」
u = np.array([[0.], [0.], [0.], [0.]]) # 外部要素

P = np.array([[0., 0., 0., 0.], [0., 0., 0., 0.], [0., 0., 100., 0.], [0., 0., 0., 100.]]) # 共分散行列
F = np.array([[1., 0., dt, 0.], [0., 1., 0., dt], [0., 0., 1., 0.], [0., 0., 0., 1.]])  # 状態遷移行列
H = np.array([[1., 0., 0, 0], [0., 1., 0., 0.]])  # 観測行列
R = np.array([[0.1, 0], [0, 0.1]]) #ノイズ
I = np.identity((len(x)))    # 4次元単位行列

def kalman_filter(x, P):
    for n in range(len(measurements)):       

        # 予測
        x = np.dot(F, x) + u
        P = np.dot(np.dot(F, P), F.T)
        
        # 計測更新
        Z = np.array([measurements[n]])
        y = Z.T - np.dot(H, x)
        S = np.dot(np.dot(H, P), H.T) + R
        K = np.dot(np.dot(P, H.T), np.linalg.inv(S))
        x = x + np.dot(K, y)        
        P = np.dot((I - np.dot(K, H)), P)
        
    x = x.tolist()
    P = P.tolist()
    return x,P


print("6回の計測後の位置と速度の予測値：",kalman_filter(x, P)[0])

例えば、上のプログラムを実行すると以下を得ます。

6回の計測後の位置と速度の予測値： [[11.993413830954994], [9.623490669593853], [9.989023051591657], [-12.294182217343579]]

得られた予測値は、6回目の観測値$[12,10]$よりわずかに下方へ修正されています。
速度は10分の1にして0.1秒単位で計算すると、x方向に約1で、y方向に約-1となっており、観測値の変化から推測できる値に近い結果が出ています。