import pylab
import math
import numpy as np


class LowPassFilter:

    def __init__(self, delta_t, ft):
        tc = 1.0 / (2 * math.pi * ft)
        self.alpha = delta_t / (delta_t + tc)
        self.y = 0

    def evaluate(self, measure):
        self.y = self.y * (1 - self.alpha) + measure * self.alpha
        return self.y


delta_t = 1e-3 # 1 ms

vx = 0.5 # pix/s
vy = 0.8 # pix/s

t = 0.0

trajectory_x = [ ]
trajectory_y = [ ]

measure_x = [ ]
measure_y = [ ]

x = 0
y = 0

error_std = 0.05

N = 0

while t < 15:

    x = x + vx * delta_t
    y = y + vy * delta_t

    trajectory_x.append(x)
    trajectory_y.append(y)

    measure_x.append(x + np.random.normal(0.0, error_std))
    measure_y.append(y + np.random.normal(0.0, error_std))

    t = t + delta_t
    N = N + 1


t = 0

filtered_trajectory_x = [ ]
filtered_trajectory_y = [ ]

CutOffFreq = 5 # Hz

fx = LowPassFilter(delta_t, CutOffFreq)
fy = LowPassFilter(delta_t, CutOffFreq)

i = 0

while t < 15:


    filtered_trajectory_x.append(fx.evaluate(measure_x[i]))
    filtered_trajectory_y.append(fy.evaluate(measure_y[i]))

    t = t + delta_t
    i = i + 1


pylab.figure(1)
pylab.plot(measure_x, measure_y, 'b-+', label='masures')
pylab.plot(trajectory_x, trajectory_y, 'r-+', label='real trajectory')
pylab.xlabel('x')
pylab.ylabel('y')
pylab.legend()

pylab.figure(2)
pylab.plot(trajectory_x, trajectory_y, 'r-+', label='real trajectory')
pylab.plot(filtered_trajectory_x, filtered_trajectory_y, 'g-+', label='fitered')
pylab.xlabel('x')
pylab.ylabel('y')
pylab.legend()

pylab.show()