-
Notifications
You must be signed in to change notification settings - Fork 0
/
mainSingle_demo.py
63 lines (48 loc) · 1.63 KB
/
mainSingle_demo.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
#import numpy as np
#import scipy as sp
#import matplotlib as mpl
#import matplotlib.pyplot as plt
from Object import Plane
from Filter import Estimate, KalmanFilter
from Sensor import NormalSensor
from Observer import KalmanrObserver
from numpy import zeros, arange, array
import matplotlib.pyplot as plt
from cluster_demo import X, P, A, Q, H, R, N_iter
estimate=Estimate(X,P)
plane=Plane('plane',X)
plane.stateSize = A.shape[0]
kf=KalmanFilter('kalmanfilter')
kf.setPosition(estimate)
radar=NormalSensor('radar')
radar.measurementSize = H.shape[0]
kfObserver=KalmanrObserver('Kalmanr Observer')
# allocate space for arrays
# Applying the Kalman Filter
for k in arange(0, N_iter):
kf.predict(A,Q)
plane.move(A)
radar.detect(plane,H,R)
kf.update(radar,H,R)
kfObserver.recieve(kf, plane, radar, k)
#reporter.result()
#import pylab
#print (Real[0,:])
#print (Real[1,:])
#pylab.figure()
#pylab.plot(kfObserver.totalReal[:,0], kfObserver.totalReal[:,1])
#pylab.plot(kfObserver.totalState[:,0], kfObserver.totalState[:,1])
#pylab.plot(kfObserver.measurement[0,0:-1], kfObserver.measurement[1,0:-1])
#pylab.show()
totalReal = kfObserver.data['real']
totalState = kfObserver.data['state']
totalMeasurement = kfObserver.data['measurement']
plt.plot(totalReal[:,0], totalReal[:,1], 'g-')
plt.plot(totalState[:,0], totalState[:,1], 'r.-')
plt.plot(totalMeasurement[:,0], totalMeasurement[:,1], 'bo')
plt.xlabel('x')
plt.ylabel('y')
plt.legend(['Real state', 'Kalman filter', 'Measurements'])
plt.title('Filter results')
#plt.savefig('Kalman.eps')
plt.show()