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));
# -----------------------------------------------------------------------------
# Simple constant value estimation
# ----------------------------------------------------------------------------
# Create the Kalman filter tracker
A = np.eye(2) # State difference equation
R = np.eye(2) * ((100**2, 0), (0, 1000**2)) # Measurement covariance
H = np.eye(2) # Measurement to state equation
P = np.eye(2) * ((100**2, 0), (0, 1000**2)) # Initial state covariance
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)
