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) ############################## kfdc.release() #sensor 1 kf1.update(radar1, H, R1) kfdc.collect(kf1, H, R1) #sensor 2 kf2.update(radar2, H, R2) kfdc.collect(kf2, H, R2) kfdc.update() # vb measurement = vstack([radar1.measurement.reshape([2,1]), radar2.measurement.reshape([2,1])]) H0 = vstack([H, H]) G0 = vstack([ hstack([getG(plane,radar1), zeros([2,2])]), hstack([zeros([2,2]),getG(plane,radar2)]) ]) R0part1 = dot(getG(plane,radar1), Rvector1).reshape([2,1]) R0part2 = dot(getG(plane,radar2), Rvector2).reshape([2,1]) R0 = vstack([ hstack([ dot(R0part1, R0part1.T), zeros([2,2]) ]), hstack([ zeros([2,2]), dot(R0part2, R0part2.T) ]) ]) vbf.update(measurement, H0, R0, G0) ############################## kfObserver1.recieve(kf1, plane, radar1, k)
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']
kf1.predict(A,Q)
#sensor 2
kf2.predict(A,Q)
#center
kfdc.predict(A,Q)
##############################
plane.move(A)
##############################
#sensor 1
radar1.detect(plane,H,R)
#sensor 2
radar2.detect(plane,H,R)
##############################
kfdc.release()
#sensor 1
kf1.update(radar1,H,R)
kfdc.collect(kf1,H,R)
#sensor 2
kf2.update(radar2,H,R)
kfdc.collect(kf2,H,R)
kfdc.update()
##############################
kfObserver1.recieve(kf1, plane, radar1, k)
kfObserver2.recieve(kf2, plane, radar2, k)
kfObserverDC.recieve(kfdc, plane, k)
##############################
#reporter.result()
#decentralized kf
#import pylab
