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));
B = np.eye(2) # State control equation # Create the filter object kalman = KalmanFilter(A, R, H, P, B) # Initialize the filter object kalman.initialize((0, 0), 0) # Setup the simulation numIter = 10000 meas = np.random.normal([5.0, 5.0], [100.0, 1000.0], (numIter, 2)) kalmanStateTrack = [0] * numIter kalmanState = np.zeros((2, numIter)) # Execute the simulation for noiseNdx in range(0, numIter): kalman.run(meas[noiseNdx, :], [0, 0]) kalmanStateTrack[noiseNdx] = copy.copy(kalman) kalmanState[:, noiseNdx] = kalman.x # Plot the results plt.plot(meas) plt.plot(kalmanState.T) # ----------------------------------------------------------------------------- # Simple 1d motion estimation # ---------------------------------------------------------------------------- # Create the Kalman filter tracker A = np.array([[1, 1], [0, 1]]) # State difference equation R = np.eye(2) * ((100**2, 0), (0, 1000**2)) # Measurement covariance H = np.eye(2) # Measurement to state equation