def update(self,measurement): ''' The update takes in account any anomaly detector if one of the model has a probability above 0.8. This feature allows the anomaly to not be taken in consideration when the system is doing a maneuver. Switching model is not breaking the estimation that way. Parameters ---------- measurement: float numpy array The measurement added to the update cycle of the filter. ''' if max(self.mu)>0.8: for filter in self.filters: filter.activate_detection() IMMEstimator.update(self,measurement)
def test_imm(): """ this test is drawn from Crassidis [1], example 4.6. ** References** [1] Crassidis. "Optimal Estimation of Dynamic Systems", CRC Press, Second edition. """ r = 1. dt = 1. phi_sim = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]]) gam = np.array([[dt**2 / 2, 0], [dt, 0], [0, dt**2 / 2], [0, dt]]) x = np.array([[2000, 0, 10000, -15.]]).T simxs = [] N = 600 for i in range(N): x = np.dot(phi_sim, x) if i >= 400: x += np.dot(gam, np.array([[.075, .075]]).T) simxs.append(x) simxs = np.array(simxs) #x = np.genfromtxt('c:/users/rlabbe/dropbox/Crassidis/mycode/x.csv', delimiter=',') zs = np.zeros((N, 2)) for i in range(len(zs)): zs[i, 0] = simxs[i, 0] + randn() * r zs[i, 1] = simxs[i, 2] + randn() * r try: #data to test against crassidis' IMM matlab code zs_tmp = np.genfromtxt( 'c:/users/rlabbe/dropbox/Crassidis/mycode/xx.csv', delimiter=',')[:-1] zs = zs_tmp except: pass ca = KalmanFilter(6, 2) cano = KalmanFilter(6, 2) dt2 = (dt**2) / 2 ca.F = np.array([[1, dt, dt2, 0, 0, 0], [0, 1, dt, 0, 0, 0], [0, 0, 1, 0, 0, 0], [0, 0, 0, 1, dt, dt2], [0, 0, 0, 0, 1, dt], [0, 0, 0, 0, 0, 1]]) cano.F = ca.F.copy() ca.x = np.array([[2000., 0, 0, 10000, -15, 0]]).T cano.x = ca.x.copy() ca.P *= 1.e-12 cano.P *= 1.e-12 ca.R *= r**2 cano.R *= r**2 cano.Q *= 0 q = np.array([[.05, .125, 1 / 6], [.125, 1 / 3, .5], [1 / 6, .5, 1] ]) * 1.e-3 ca.Q[0:3, 0:3] = q ca.Q[3:6, 3:6] = q ca.H = np.array([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]]) cano.H = ca.H.copy() filters = [ca, cano] trans = np.array([[0.97, 0.03], [0.03, 0.97]]) bank = IMMEstimator(filters, (0.5, 0.5), trans) # ensure __repr__ doesn't have problems str(bank) xs, probs = [], [] cvxs, caxs = [], [] s = Saver(bank) for i, z in enumerate(zs[0:10]): z = np.array([z]).T bank.update(z) #print(ca.likelihood, cano.likelihood) #print(ca.x.T) xs.append(bank.x.copy()) cvxs.append(ca.x.copy()) caxs.append(cano.x.copy()) #print(i, ca.likelihood, cano.likelihood, bank.w) #print('p', bank.p) probs.append(bank.mu.copy()) s.save() s.to_array() if DO_PLOT: xs = np.array(xs) cvxs = np.array(cvxs) caxs = np.array(caxs) probs = np.array(probs) plt.subplot(121) plt.plot(xs[:, 0], xs[:, 3], 'k') #plt.plot(cvxs[:, 0], caxs[:, 3]) #plt.plot(simxs[:, 0], simxs[:, 2], 'g') plt.scatter(zs[:, 0], zs[:, 1], marker='+', alpha=0.2) plt.subplot(122) plt.plot(probs[:, 0]) plt.plot(probs[:, 1]) plt.ylim(-1.5, 1.5) plt.title('probability ratio p(cv)/p(ca)') '''plt.figure() plt.plot(cvxs, label='CV') plt.plot(caxs, label='CA') plt.plot(xs[:, 0], label='GT') plt.legend() plt.figure() plt.plot(xs) plt.plot(xs[:, 0])''' return bank
def test_imm(): """ this test is drawn from Crassidis [1], example 4.6. ** References** [1] Crassidis. "Optimal Estimation of Dynamic Systems", CRC Press, Second edition. """ dt = 0.1 pos, zs = generate_data(120, noise_factor=0.6) z_xs = zs[:, 0] t = np.arange(0, len(z_xs) * dt, dt) dt = 0.1 ca = make_ca_filter(dt, noise_factor=0.6) cv = make_ca_filter(dt, noise_factor=0.6) cv.F[:,2] = 0 # remove acceleration term cv.P[2,2] = 0 cv.Q[2,2] = 0 filters = [cv, ca] trans = np.array([[0.97, 0.03], [0.03, 0.97]]) trans = np.array([[0.8, 0.2], [0.05, 0.95]]) bank = IMMEstimator(filters, (0.5, 0.5), trans, dim_x=3) xs, probs = [], [] cvxs, caxs = [], [] for i, z in enumerate(z_xs): bank.update(z) xs.append(bank.x[0]) cvxs.append(cv.x[0]) caxs.append(ca.x[0]) #print(i, cv.likelihood, ca.likelihood, bank.w) #print('p', bank.p) probs.append(bank.w[0] / bank.w[1]) if DO_PLOT: plt.subplot(121) plt.plot(xs) plt.plot(pos[:, 0]) plt.subplot(122) plt.plot(probs) plt.title('probability ratio p(cv)/p(ca)') plt.figure() plt.plot(cvxs, label='CV') plt.plot(caxs, label='CA') plt.plot(pos[:, 0], label='GT') plt.legend() plt.figure() plt.plot(xs) plt.plot(pos[:, 0])
trans = np.array([[0.97, 0.03], [0.03, 0.97]]) bank = IMMEstimator((6, 1), filters, (0.5, 0.5), trans) xs, probs = [], [] cvxs, caxs = [], [] for i, z in enumerate(zs): print("\ni=", i+1) if i == 10000: break #print(z) z = np.array([z]).T bank.update(z) #print(ca.likelihood, cano.likelihood) #print(ca.x.T) xs.append(bank.x.copy()) cvxs.append(ca.x.copy()) caxs.append(cano.x.copy()) #print(i, ca.likelihood, cano.likelihood, bank.w) #print('p', bank.p) probs.append(bank.w.copy()) DO_PLOT = True if DO_PLOT: xs = np.array(xs) cvxs = np.array(cvxs) caxs = np.array(caxs)
def test_imm(): """ this test is drawn from Crassidis [1], example 4.6. ** References** [1] Crassidis. "Optimal Estimation of Dynamic Systems", CRC Press, Second edition. """ r = 1. dt = 1. phi_sim = np.array( [[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]]) gam = np.array([[dt**2/2, 0], [dt, 0], [0, dt**2/2], [0, dt]]) x = np.array([[2000, 0, 10000, -15.]]).T simxs = [] N = 600 for i in range(N): x = np.dot(phi_sim, x) if i >= 400: x += np.dot(gam, np.array([[.075, .075]]).T) simxs.append(x) simxs = np.array(simxs) #x = np.genfromtxt('c:/users/rlabbe/dropbox/Crassidis/mycode/x.csv', delimiter=',') zs = np.zeros((N, 2)) for i in range(len(zs)): zs[i, 0] = simxs[i, 0] + randn()*r zs[i, 1] = simxs[i, 2] + randn()*r try: #data to test against crassidis' IMM matlab code zs_tmp = np.genfromtxt('c:/users/rlabbe/dropbox/Crassidis/mycode/xx.csv', delimiter=',')[:-1] zs = zs_tmp except: pass ca = KalmanFilter(6, 2) cano = KalmanFilter(6, 2) dt2 = (dt**2)/2 ca.F = np.array( [[1, dt, dt2, 0, 0, 0], [0, 1, dt, 0, 0, 0], [0, 0, 1, 0, 0, 0], [0, 0, 0, 1, dt, dt2], [0, 0, 0, 0, 1, dt], [0, 0, 0, 0, 0, 1]]) cano.F = ca.F.copy() ca.x = np.array([[2000., 0, 0, 10000, -15, 0]]).T cano.x = ca.x.copy() ca.P *= 1.e-12 cano.P *= 1.e-12 ca.R *= r**2 cano.R *= r**2 cano.Q *= 0 q = np.array([[.05, .125, 1/6], [.125, 1/3, .5], [1/6, .5, 1]])*1.e-3 ca.Q[0:3, 0:3] = q ca.Q[3:6, 3:6] = q ca.H = np.array([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]]) cano.H = ca.H.copy() filters = [ca, cano] trans = np.array([[0.97, 0.03], [0.03, 0.97]]) bank = IMMEstimator(filters, (0.5, 0.5), trans) xs, probs = [], [] cvxs, caxs = [], [] for i, z in enumerate(zs[0:10]): z = np.array([z]).T bank.update(z) #print(ca.likelihood, cano.likelihood) #print(ca.x.T) xs.append(bank.x.copy()) cvxs.append(ca.x.copy()) caxs.append(cano.x.copy()) #print(i, ca.likelihood, cano.likelihood, bank.w) #print('p', bank.p) probs.append(bank.mu.copy())
def test_imm(): """ this test is drawn from Crassidis [1], example 4.6. ** References** [1] Crassidis. "Optimal Estimation of Dynamic Systems", CRC Press, Second edition. """ r = 1. dt = 1. phi_sim = np.array( [[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]]) gam = np.array([[dt**2/2, 0], [dt, 0], [0, dt**2/2], [0, dt]]) x = np.array([[2000, 0, 10000, -15.]]).T simxs = [] N = 600 for i in range(N): x = np.dot(phi_sim, x) if i >= 400: x += np.dot(gam, np.array([[.075, .075]]).T) simxs.append(x) simxs = np.array(simxs) #x = np.genfromtxt('c:/users/rlabbe/dropbox/Crassidis/mycode/x.csv', delimiter=',') zs = np.zeros((N, 2)) for i in range(len(zs)): zs[i, 0] = simxs[i, 0] + randn()*r zs[i, 1] = simxs[i, 2] + randn()*r try: #data to test against crassidis' IMM matlab code zs_tmp = np.genfromtxt('c:/users/rlabbe/dropbox/Crassidis/mycode/xx.csv', delimiter=',')[:-1] zs = zs_tmp except: pass ca = KalmanFilter(6, 2) cano = KalmanFilter(6, 2) ca.F = np.array( [[1, dt, dt**2/2, 0, 0, 0], [0, 1, dt, 0, 0, 0], [0, 0, 1, 0, 0, 0], [0, 0, 0, 1, dt, dt**2/2], [0, 0, 0, 0, 1, dt], [0, 0, 0, 0, 0, 1]]) cano.F = ca.F.copy() ca.x = np.array([[2000., 0, 0, 10000, -15, 0]]).T cano.x = ca.x.copy() ca.P *= 1.e-12 cano.P *= 1.e-12 ca.R *= r**2 cano.R *= r**2 cano.Q *= 0 q = np.array([[.05, .125, .16666666666666666666666666667], [.125, .333333333333333333333333333333333333, .5], [.166666666666666666666666667, .5, 1]])*1.e-3 ca.Q[0:3, 0:3] = q ca.Q[3:6, 3:6] = q ca.H = np.array([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]]) cano.H = ca.H.copy() filters = [ca, cano] trans = np.array([[0.8, 0.2], [0.05, 0.95]]) trans = np.array([[0.97, 0.03], [0.03, 0.97]]) bank = IMMEstimator(filters, (0.5, 0.5), trans) xs, probs = [], [] cvxs, caxs = [], [] for i, z in enumerate(zs): z = np.array([z]).T bank.update(z) #print(ca.likelihood, cano.likelihood) #print(ca.x.T) xs.append(bank.x.copy()) cvxs.append(ca.x.copy()) caxs.append(cano.x.copy()) #print(i, ca.likelihood, cano.likelihood, bank.w) #print('p', bank.p) probs.append(bank.mu.copy()) if DO_PLOT: xs = np.array(xs) cvxs = np.array(cvxs) caxs = np.array(caxs) probs = np.array(probs) plt.subplot(121) plt.plot(xs[:, 0], xs[:, 3], 'k') #plt.plot(cvxs[:, 0], caxs[:, 3]) #plt.plot(simxs[:, 0], simxs[:, 2], 'g') plt.scatter(zs[:, 0], zs[:, 1], marker='+', alpha=0.2) plt.subplot(122) plt.plot(probs[:, 0]) plt.plot(probs[:, 1]) plt.ylim(-1.5, 1.5) plt.title('probability ratio p(cv)/p(ca)') '''plt.figure()
def test_imm(): """ This test is drawn from Crassidis [1], example 4.6. ** References** [1] Crassidis. "Optimal Estimation of Dynamic Systems", CRC Press, Second edition. """ r = 100. dt = 1. phi_sim = np.array( [[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]]) gam = np.array([[dt**2/2, 0], [dt, 0], [0, dt**2/2], [0, dt]]) x = np.array([[2000, 0, 10000, -15.]]).T simxs = [] N = 600 for i in range(N): x = np.dot(phi_sim, x) if i >= 400: x += np.dot(gam, np.array([[.075, .075]]).T) simxs.append(x) simxs = np.array(simxs) zs = np.zeros((N, 2)) for i in range(len(zs)): zs[i, 0] = simxs[i, 0] + randn()*r zs[i, 1] = simxs[i, 2] + randn()*r ''' try: # data to test against crassidis' IMM matlab code zs_tmp = np.genfromtxt('c:/users/rlabbe/dropbox/Crassidis/mycode/xx.csv', delimiter=',')[:-1] zs = zs_tmp except: pass ''' ca = KalmanFilter(6, 2) cano = KalmanFilter(6, 2) dt2 = (dt**2)/2 ca.F = np.array( [[1, dt, dt2, 0, 0, 0], [0, 1, dt, 0, 0, 0], [0, 0, 1, 0, 0, 0], [0, 0, 0, 1, dt, dt2], [0, 0, 0, 0, 1, dt], [0, 0, 0, 0, 0, 1]]) cano.F = ca.F.copy() ca.x = np.array([[2000., 0, 0, 10000, -15, 0]]).T cano.x = ca.x.copy() ca.P *= 1.e-12 cano.P *= 1.e-12 ca.R *= r**2 cano.R *= r**2 cano.Q *= 0 q = np.array([[.05, .125, 1./6], [.125, 1/3, .5], [1./6, .5, 1.]])*1.e-3 ca.Q[0:3, 0:3] = q ca.Q[3:6, 3:6] = q ca.H = np.array([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]]) cano.H = ca.H.copy() filters = [ca, cano] trans = np.array([[0.97, 0.03], [0.03, 0.97]]) bank = IMMEstimator(filters, (0.5, 0.5), trans) # ensure __repr__ doesn't have problems str(bank) s = Saver(bank) ca_s = Saver(ca) cano_s = Saver(cano) for i, z in enumerate(zs): z = np.array([z]).T bank.update(z) bank.predict() s.save() ca_s.save() cano_s.save() if DO_PLOT: s.to_array() ca_s.to_array() cano_s.to_array() plt.figure() plt.subplot(121) plt.plot(s.x[:, 0], s.x[:, 3], 'k') #plt.plot(cvxs[:, 0], caxs[:, 3]) #plt.plot(simxs[:, 0], simxs[:, 2], 'g') plt.scatter(zs[:, 0], zs[:, 1], marker='+', alpha=0.2) plt.subplot(122) plt.plot(s.mu[:, 0]) plt.plot(s.mu[:, 1]) plt.ylim(0, 1) plt.title('probability ratio p(cv)/p(ca)') '''plt.figure()
class Tracker_SingleTarget_MultipleModel(object): """ """ def __init__(self, deltaT, measurementNoiseStd): self.updatedPredictions = [] self.mus = [] """ First init constant linear model """ points1 = MerweScaledSigmaPoints(5, alpha=0.0025, beta=2., kappa=0) self.constantLinearModel = UnscentedKalmanFilter( dim_x=5, dim_z=2, dt=deltaT, fx=f_unscented_linearModel, hx=h_unscented_linearModel, points=points1) self.constantLinearModel.x = np.array([0.01, 0.01, 0.01, 0.01, 0]) self.constantLinearModel.P = np.eye(5) * (measurementNoiseStd**2) / 2.0 self.constantLinearModel.R = np.eye(2) * (measurementNoiseStd**2) self.constantLinearModel.Q = np.diag([0.003, 0.003, 6e-4, 0.004, 0]) """ Second init constant turn rate model """ points2 = MerweScaledSigmaPoints(5, alpha=0.0025, beta=2., kappa=0) self.constantTurnRateModel = UnscentedKalmanFilter( dim_x=5, dim_z=2, dt=deltaT, fx=f_unscented_turnRateModel, hx=h_unscented_turnRateModel, points=points2) self.constantTurnRateModel.x = np.array( [0.01, 0.01, 0.01, 0.001, 1e-5]) self.constantTurnRateModel.P = np.eye(5) * (measurementNoiseStd** 2) / 2.0 self.constantTurnRateModel.R = np.eye(2) * (measurementNoiseStd**2) self.constantTurnRateModel.Q = np.diag( [1e-24, 1e-24, 1e-3, 4e-3, 1e-10]) """ Third init random motion model """ points3 = MerweScaledSigmaPoints(5, alpha=0.0025, beta=2., kappa=0) self.randomModel = UnscentedKalmanFilter(dim_x=5, dim_z=2, dt=deltaT, fx=f_unscented_randomModel, hx=h_unscented_randomModel, points=points3) self.randomModel.x = np.array([0.01, 0.01, 0.01, 0.001, 1e-5]) self.randomModel.P = np.eye(5) * (measurementNoiseStd**2) / 2.0 self.randomModel.R = np.eye(2) * (measurementNoiseStd**2) self.randomModel.Q = np.diag([1, 1, 1e-24, 1e-24, 1e-24]) #############################33 if (1): filters = [self.constantLinearModel, self.constantTurnRateModel] mu = [0.5, 0.5] trans = np.array([[0.9, 0.1], [0.1, 0.9]]) self.imm = IMMEstimator(filters, mu, trans) else: filters = [ self.constantLinearModel, self.constantTurnRateModel, self.randomModel ] mu = [0.34, 0.33, 0.33] trans = np.array([[0.9, 0.05, 0.05], [0.05, 0.9, 0.05], [0.05, 0.05, 0.9]]) self.imm = IMMEstimator(filters, mu, trans) def predictAndUpdate(self, measurement): self.imm.P = 1 / 2.0 * (self.imm.P + self.imm.P.T) self.imm.predict() self.imm.update(measurement) self.updatedPredictions.append( np.array((self.imm.x_post[0], self.imm.x_post[1]))) self.mus.append(self.imm.mu)
lastul = (0, 0) lastbr = (0, 0) fourcc = cv2.VideoWriter_fourcc(*'mp4v') videof = cv2.VideoWriter('video.avi', fourcc, 1, (1000, 500)) while True: start_time = time.time() # start time of the loop frame = bg_image.copy() x, y = win32api.GetCursorPos() x = x - window_ul[0] y = y - window_ul[1] cv2.rectangle(frame, (x - boxhsize[0], y - boxhsize[1]), (x + boxhsize[0], y + boxhsize[1]), (0, 255, 0), 3) z = np.array([[x], [y]]) imm.update(z) imm.predict() print(imm.x.T) # if order==1: # xp=imm.x_prior[0] # yp=imm.x_prior[2] # elif order==2: # xp=imm.x_prior[0] # yp=imm.x_prior[3] new_vals = imm.x.T[0] if order == 1: xp = int(new_vals[0]) yp = int(new_vals[2]) elif order == 2: xp = int(new_vals[0])