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']
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) kfObserver2.recieve(kf2, plane, radar2, k) kfObserverDC.recieve(kfdc, plane, k) vbObserverDC.recieve(vbf, plane, k) ############################## totalReal1 = kfObserver1.data['real'] totalState1 = kfObserver1.data['state'] totalMeasurement1 = kfObserver1.data['measurement'] plt.plot(totalReal1[:,0], totalReal1[:,2], 'g-') plt.plot(totalState1[:,0], totalState1[:,2], 'y.-') plt.plot(totalMeasurement1[:,0], totalMeasurement1[:,1], 'bo') #hold('on') totalState2 = kfObserver2.data['state']