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()
kfdc=KalmanFilterDecentrailized('Kalman filter Centralized')
kfdc.setPosition(estimate)
kfObserverDC=KalmanrObserverDecentralized('Kalmanr Observer for center')
# allocate space for arrays
kfdc=KalmanFilterDecentrailized('Kalman filter Centralized')
kfdc.setPosition(estimate)
kfObserverDC=KalmanrObserverDecentralized('Kalmanr Observer for center')
# variational filtering
vbf = VBbiasFilter('Variational Bayesian Filtering')
vbf.setPosition(biasEstimate)
vbObserverDC=VBbiasObserverDecentralized('VB bias Observer Decentralized')
# Applying the Kalman Filter
for k in arange(0, N_iter):
##############################
#sensor 1
kf1.predict(A, Q)
#sensor 2
kf2.predict(A,Q)
#center
kfdc.predict(A, Q)
#vb
vbf.calcUncertain()
vbf.timeEvolution(1)
vbf.predict(A, Q, E)
##############################
plane.move(A, Q)
##############################
#sensor 1
radar1.detect(plane, H, R1)
#sensor 2
radar2.detect(plane, H, R2)
