Python KalmanFilter.update Examples

Example #1

File: mainMulti_SR_demo.py Project: junlongsun/PyKalmanFilter

 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)

Example #2

File: mainSingle_demo.py Project: junlongsun/PyKalmanFilter

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']

Example #3

File: mainMulti_demo.py Project: junlongsun/PyKalmanFilter

    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