48 lines
751 B
Python
48 lines
751 B
Python
|
# -*- coding: utf-8 -*-
|
||
|
|
"""
|
||
|
|
Created on Sat May 24 14:42:55 2014
|
||
|
|
|
||
|
|
@author: rlabbe
|
||
|
|
"""
|
||
|
|
|
||
|
|
from KalmanFilter import KalmanFilter
|
||
|
|
import numpy as np
|
||
|
|
import matplotlib.pyplot as plt
|
||
|
|
import numpy.random as random
|
||
|
|
|
||
|
|
f = KalmanFilter (dim=4)
|
||
|
|
|
||
|
|
dt = 1
|
||
|
|
f.F = np.mat ([[1, dt, 0, 0],
|
||
|
|
[0, 1, 0, 0],
|
||
|
|
[0, 0, 1, dt],
|
||
|
|
[0, 0, 0, 1]])
|
||
|
|
|
||
|
|
f.H = np.mat ([[1, 0, 0, 0],
|
||
|
|
[0, 0, 1, 0]])
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
f.Q *= 4.
|
||
|
|
f.R = np.mat([[50,0],
|
||
|
|
[0, 50]])
|
||
|
|
|
||
|
|
f.x = np.mat([0,0,0,0]).T
|
||
|
|
f.P *= 100.
|
||
|
|
|
||
|
|
|
||
|
|
xs = []
|
||
|
|
ys = []
|
||
|
|
count = 200
|
||
|
|
for i in range(count):
|
||
|
|
z = np.mat([[i+random.randn()*1],[i+random.randn()*1]])
|
||
|
|
f.predict ()
|
||
|
|
f.update (z)
|
||
|
|
xs.append (f.x[0,0])
|
||
|
|
ys.append (f.x[2,0])
|
||
|
|
|
||
|
|
|
||
|
|
plt.plot (xs, ys)
|
||
|
|
plt.show()
|
||
|
|
|