kalman.py

# -*- coding: utf-8 -*-
# @Author: juliangaal
# @Date:   2018-02-10 12:25:41
# @Last Modified by:   juliangaal
# @Last Modified time: 2018-02-10 15:07:28
import numpy as np
import matplotlib.pyplot as plt

values   = [1124, 800, 628, 379, 199, 104, 279, 227, 97, 226, 105, 95, 150, 180, 200, 226, 250, 279, 399, 415, 430, 455, 470, 498, 504]
filtered = []

def filter():
    global values
    
    a = 0.75
    u = 1
    b = 0.5
    c = 1
    r = 2
    xhat = values[0]
    pred = gain = 1

    for z in values:
        # Update
        xhat = a * xhat + b * u
        pred = a * pred * a

        # Predict 
        gain = pred * c / (c * pred * c + r)
        xhat = xhat + gain * (z - c * xhat)
        pred = (1 - gain * c) * pred 

        # save
        filtered.append(xhat)       

filter()
x = np.linspace(0, len(values) + 1, len(values))
plt.plot(x, filtered, label='filtered')
plt.plot(x, values,   label='original')
plt.show()