Pythonでカルマンフィルタを使ってみた
この記事では、Pythonでカルマンフィルタを使った実験を紹介する。pykalmanライブラリを使って、ノイズの混じった正弦波にカルマンフィルタをかけてみた。また、FIRローパスフィルタとの比較も行った。
なお、動作環境は次の通りである。
項目 | バージョン |
---|---|
PC | macOS Big Sur version 11.0.1 |
Python | 3.7.3 |
pykalmanのインストール
今回は、pykalmanというカルマンフィルタライブラリを使うので次の通りインストールしておく。また、scipyも必要とされるので同時にインストールしておく。
shell
$ pip3 install pykalman
$ pip3 install scipy
pykalman—pykalman0.9.2documentation
Pythonでカルマンフィルタ
今回の実験では、下のプログラムを使った。正弦波にノイズをかけたデータを用意し、自作のローパスフィルタ(FIR)とカルマンフィルタを通してグラフで比較するプログラムである。
py
# Created by Toshihiko Arai.
# https://101010.fun/programming/python-kalman.html
from matplotlib import pyplot as plt
from random import randint
import math
from pykalman import KalmanFilter
import numpy as np
def sine_wave(samplerate, frequency):
x = list(range(samplerate))
y = [math.sin(i*2*math.pi/samplerate*frequency) for i in x]
return x, y
def add_noise(values):
noise = [randint(-100, 100) * 0.01 for _ in values]
y = [w + n for (w, n) in zip(values, noise)]
return y
def filtered_kalman(values):
kf = KalmanFilter(transition_matrices=np.array([[1, 1], [0, 1]]),
transition_covariance=0.0001 * np.eye(2)) # np.eyeは単位行列
smoothed = kf.em(values).smooth(values)[0]
filtered = kf.em(values).filter(values)[0]
return smoothed, filtered
def filtered_lowpass(values):
res = [0 for _ in values]
k = 0.2
i = 0
for a in values:
try:
b = res[i - 1]
except:
b = 0
res[i] = k * a + (1-k) * b
i += 1
return res
if __name__ == '__main__':
x, sine_y = sine_wave(500, 5)
noised_y = add_noise(sine_y)
smoothed, filtered = filtered_kalman(noised_y)
lowpass_y = filtered_lowpass(noised_y)
plt.figure(figsize=(16, 9), dpi=80)
plt.plot(x, sine_y, label='Original')
plt.plot(x, noised_y, label='Noised')
plt.plot(x, lowpass_y, label='FIR LPF')
plt.plot(x, smoothed[:, 0], label='Kalman Smoothed')
# plt.plot(x, filtered[:, 0], label='Filtered')
plt.legend()
plt.show()
実験結果
正弦波にノイズを足したグラフ
正弦波(青)にノイズを足したもの(オレンジ)が、フィルタの対象となるデータである。
自作ローパスフィルタを通したグラフ
ローパスフィルタだと、ノイズは完全に除去できない。強めにかけると振幅が小さくなってしまったり、また位相もズレてしまう。
カルマンフィルタを通したグラフ
今回はじめてカルマンフィルタを使ってみたが、原型の正弦波にかなり近い結果になり驚いた。
カルマンフィルタとローパスフィルタの比較
このように、ローパスフィルタよりもカルマンフィルタの方が原型に近い正弦波を取り出すことができる。