カルマンフィルター

カルマンフィルターとは？

カルマンフィルターは、不確実性を伴う動的システムの状態を推定するためのアルゴリズムです。このフィルターは、ノイズのある観測データと数学的なモデルを組み合わせて、システムの真の状態をできるだけ正確に推定することを目指します。

この記事では、filterpyライブラリを使ってカルマンフィルターを実装してみます。実際に測定値にランダムなノイズが加わっている状況を想定し、カルマンフィルターでそのノイズを除去しながら真の位置を推定します。

インストール

pip install filterpy

カルマンフィルターを初期化

import numpy as np
from filterpy.kalman import KalmanFilter

def setup_kalman_filter():
    kf = KalmanFilter(dim_x=2, dim_z=1)
    # 初期状態
    kf.x = np.array([0., 1.]) 
    # 状態遷移行列
    dt = 1.  # 時間ステップ (秒)
    kf.F = np.array([[1, dt],
                     [0, 1]])
    # 測定行列
    kf.H = np.array([[1., 0]])
    # プロセスノイズ共分散行列
    kf.Q = np.array([[0.01, 0.1],
                     [0.1, 1.]])
    # 測定ノイズ共分散行列
    kf.R = np.array([[5.]]) 
    # 推定誤差共分散行列
    kf.P *= 100.
    
    return kf
    
kf = setup_kalman_filter()

• dim_x=2：状態ベクトルの次元を2次元に指定。通常、位置と速度を表すために使用します。

• dim_z=1：測定ベクトルの次元を1次元に指定。この例では位置のみが測定されます。

true_positions = np.linspace(0, 60, 61)  # 60秒間で60m移動
measured_positions = true_positions + np.random.normal(0, 2, size=len(true_positions))  # ノイズを加えた測定値

一定速度で移動する物体の真の位置データを生成し、そこにランダムなノイズを加えた測定値を生成しています。

estimated_positions = []
for measurement in measured_positions:
    kf.predict() 
    kf.update(measurement) 
    estimated_positions.append(kf.x[0])

ノイズを含む測定値を使ってカルマンフィルタを実行し、位置の推定値を逐次更新してリストに保存します。

可視化

import matplotlib.pyplot as plt

plt.figure(figsize=(10, 5))
plt.plot(true_positions, label='True Position')
plt.plot(measured_positions, label='Measured Position', linestyle='dotted')
plt.plot(estimated_positions, label='Estimated Position', linestyle='--')
plt.xlabel('Time (seconds)')
plt.ylabel('Position')
plt.legend()
plt.title('Kalman Filter for Position Estimation')
plt.show()

真の位置、測定位置、およびカルマンフィルターによる推定位置を視覚化しています。