def run(self): # Create the Kalman filter tracker A = np.eye(6) deltaT = np.array(2e-3) A2 = np.eye(6, k=3) * deltaT A = A + A2 R = np.eye(3) * 100**2 H = np.eye(3, 6) P = np.eye(6) * 100**2 B = np.eye(6) kalman = KalmanFilter(A, R, H, P, B) first = 0 oldDataNdx = np.zeros(1, dtype=np.uint64) tmp = np.array(np.zeros(6)) while (1): navData = navQueue.get() dataNdx = navQueue.get() numMeas = np.size(navData, 1) if first == 0: kalman.initialize(np.hstack((navData[0:3, 0], 0, 0, 0))) # Loop over each position measurement for measNdx in range(0, numMeas): if measNdx == 0 and first != 0: deltaT = np.array(np.float(dataNdx - oldDataNdx) * 1e-3) A2 = np.eye(6, k=3) * deltaT kalman.A = A + A2 else: deltaT = np.array(1e-3) A2 = np.eye(6, k=3) * deltaT kalman.A = A + A2 kalman.run(navData[0:3, measNdx]) tmp = np.vstack((tmp, kalman.x)) print('%i' % (dataNdx - oldDataNdx)) oldDataNdx = deepcopy(dataNdx) navQueue.task_done() navQueue.task_done() first = first + 1 # Convert from ECEF to lat/lon (lat, lon, height) = ecef2lla(kalman.x[0], kalman.x[1], kalman.x[2]) mapQueue.put((lat, lon))
def run(self): # Create the Kalman filter tracker A = np.eye(6); deltaT = np.array(2e-3); A2 = np.eye(6,k=3) * deltaT; A = A + A2; R = np.eye(3) * 100**2; H = np.eye(3,6); P = np.eye(6) * 100**2; B = np.eye(6); kalman = KalmanFilter(A,R,H,P,B); first = 0; oldDataNdx = np.zeros(1,dtype=np.uint64); tmp = np.array(np.zeros(6)); while(1): navData = navQueue.get(); dataNdx = navQueue.get(); numMeas = np.size(navData,1); if first == 0: kalman.initialize( np.hstack( (navData[0:3,0],0,0,0) ) ); # Loop over each position measurement for measNdx in range(0,numMeas): if measNdx == 0 and first != 0: deltaT = np.array(np.float(dataNdx - oldDataNdx)*1e-3); A2 = np.eye(6,k=3) * deltaT; kalman.A = A + A2; else: deltaT = np.array(1e-3); A2 = np.eye(6,k=3) * deltaT; kalman.A = A + A2; kalman.run( navData[0:3,measNdx] ); tmp = np.vstack((tmp,kalman.x)); print('%i' % (dataNdx - oldDataNdx)); oldDataNdx = deepcopy(dataNdx); navQueue.task_done(); navQueue.task_done(); first = first + 1; # Convert from ECEF to lat/lon (lat, lon, height) = ecef2lla(kalman.x[0],kalman.x[1],kalman.x[2]); mapQueue.put((lat,lon));