Pythonでカルマンフィルタを使ってみた

カルマンフィルタを通したグラフ

この記事では、Pythonでカルマンフィルタを使った実験を紹介する。pykalmanライブラリを使って、ノイズの混じった正弦波にカルマンフィルタをかけてみた。また、FIRローパスフィルタとの比較も行った。

なお、動作環境は次の通りである。

項目バージョン
PCmacOS Big Sur version 11.0.1
Python3.7.3

pykalmanのインストール

今回は、pykalmanというカルマンフィルタライブラリを使うので次の通りインストールしておく。また、scipyも必要とされるので同時にインストールしておく。

$ pip3 install pykalman
$ pip3 install scipy

Pythonでカルマンフィルタ

今回の実験では、下のプログラムを使った。正弦波にノイズをかけたデータを用意し、自作のローパスフィルタ(FIR)とカルマンフィルタを通してグラフで比較するプログラムである。

# 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()

実験結果

正弦波にノイズを足したグラフ

正弦波にノイズを足したグラフ
正弦波にノイズを足したグラフ

正弦波(青)にノイズを足したもの(オレンジ)が、フィルタの対象となるデータである。

自作ローパスフィルタを通したグラフ

自作ローパスフィルタを通したグラフ
自作ローパスフィルタを通したグラフ

ローパスフィルタだと、ノイズは完全に除去できない。強めにかけると振幅が小さくなってしまったり、また位相もズレてしまう。

カルマンフィルタを通したグラフ

カルマンフィルタを通したグラフ
カルマンフィルタを通したグラフ

今回はじめてカルマンフィルタを使ってみたが、このようにかなり原型の正弦波に近い結果に驚いた。

カルマンフィルタとローパスフィルタの比較

![カルマンフィルタとローパスフィルタの比較](python-kalman-and-lpf.jpg "1000")このように、ローパスフィルタよりもカルマンフィルタの方が原型に近い正弦波を取り出すことができる。

最後まで読んでいただきありがとうございました。

「この記事が参考になったよ」という方は、ぜひ記事をシェアをしていただけるととても嬉しいです。

今後も有益な記事を書くモチベーションにつながりますので、どうかよろしくお願いいたします。↓↓↓↓↓↓↓

あなたにおすすめ