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_misshapen(): """Ensure we get a ValueError if the filter banks are not designed properly """ ca = KalmanFilter(3, 1) cv = KalmanFilter(2, 1) trans = np.array([[0.97, 0.03], [0.03, 0.97]]) try: IMMEstimator([ca, cv], (0.5, 0.5), trans) assert "IMM should raise ValueError on filter banks with filters of different sizes" except ValueError: pass try: IMMEstimator([], (0.5, 0.5), trans) assert "Should raise ValueError on empty bank" except ValueError: pass
def __init__(self, pfa=1e-6, sn0=50, beamwidth=0.02, n_max=20, x0=np.array([30e3, -150, 0, 30e3, 150, 0]), theta_accuracy=0.002): # Trackers probs = np.ones(2) / 2 p_switch = 1.0 / 1000.0 M = np.array([ [1 - p_switch, p_switch], [p_switch, 1 - p_switch]]) F_ca = kinematic_state_transition(self.DT, self.ORDER, self.DIM) F_cv = kinematic_state_transition(self.DT, self.ORDER, self.DIM) F_cv[:, 2::3] = 0 g = 9.81 var = 4 * g ** 2 Q = filterpy.common.Q_discrete_white_noise(self.DIM, self.DT, var, block_size=self.ORDER + 1) kf_cv = KalmanFilter(dim_x=self.DIM * (self.ORDER + 1), dim_z=self.DIM) kf_cv.F = F_cv kf_cv.Q = Q kf_cv.H = position_measurement_matrix(self.DIM, self.ORDER) kf_ca = KalmanFilter(dim_x=self.DIM * (self.ORDER + 1), dim_z=self.DIM) kf_ca.F = F_ca kf_ca.Q = Q kf_ca.H = position_measurement_matrix(self.DIM, self.ORDER) filters = [kf_cv, kf_ca] tracker = IMMEstimator(filters, probs, M) # Target st_models = { 0: F_cv, 1000: F_ca, 2000: F_cv } p_noises = { 0: Q } target = DefinedTargetProcess(x0, st_models, p_noises, self.MAX_STEPS, self.ORDER, self.DIM) # Radar radar = PositionRadar(target, sn0, pfa, beamwidth, self.DIM, self.ORDER, angle_accuracy=theta_accuracy) # Computer P0 = np.zeros((x0.size,) * 2) computer = TrackingComputer(tracker, radar, n_max, P0=P0) super().__init__(computer)
def __init__(self, var_cv=737.5, var_ca=73.7, p_switch=0.028, traj_idx=0): F_ca = kinematic_state_transition(self.DT, self.ORDER, self.DIM) F_cv = F_ca.copy() F_cv[:, 2::3] = 0 G = np.array([[1 / 2 * self.DT**2, self.DT, 1]]).T A = G @ G.T Q_ca = block_diag(A, A) * var_ca Q_cv = block_diag(A, A) * var_cv Q_cv[:, 2::3] = 0 Q_cv[2::3, :] = 0 kf_ca = KalmanFilter(dim_x=self.DIM * (self.ORDER + 1), dim_z=self.DIM) kf_ca.F = F_ca kf_ca.Q = Q_ca kf_ca.H = position_measurement_matrix(self.DIM, self.ORDER) kf_cv = KalmanFilter(dim_x=self.DIM * (self.ORDER + 1), dim_z=self.DIM) kf_cv.F = F_cv kf_cv.Q = Q_cv kf_cv.H = position_measurement_matrix(self.DIM, self.ORDER) filters = [kf_cv, kf_ca] mu = [0.5, 0.5] M = np.array([[1 - p_switch, p_switch], [p_switch, 1 - p_switch]]) tracker = IMMEstimator(filters, mu, M) target = load_benchmark_target(traj_idx, self.ORDER, self.DIM, skip_k=10) sn0 = 50.0 pfa = 1e-6 beamwidth = 0.02 radar = PositionRadar(target, sn0, pfa, beamwidth, self.DIM, self.ORDER) n_max = 20 P0 = np.zeros((6, 6), dtype=float) target.reset() x0 = target.x computer = TrackingComputer(tracker, radar, n_max, x0=x0, P0=P0) super().__init__(computer=computer)
def __init__(self, pfa=1e-6, sn0=50, beamwidth=0.02, n_max=20, x0=np.array([10e3, -200.0, 10e3, 0]), P0=np.eye(4) * 1000, theta_accuracy=0.002): var = 0.1*9.81**2 tr1 = -0.08 tr2 = 0.1 probs = np.ones(3) / 3 p_switch = 1.0 / 100.0 M = np.array([ [1 - p_switch, p_switch / 2, p_switch / 2], [p_switch / 2, 1 - p_switch, p_switch / 2], [p_switch / 2, p_switch / 2, 1 - p_switch] ]) filters = list() for i in range(3): filters.append(filterpy.common.kinematic_kf(self.DIM, self.ORDER, self.DT)) filters[i].x = x0 filters[i].Q = filterpy.common.Q_discrete_white_noise(self.DIM, self.DT, var=var, block_size=self.ORDER+1) filters[1].F = constant_turn_rate_matrix(tr1, self.DT) filters[2].F = constant_turn_rate_matrix(tr2, self.DT) st_models = { 0: kinematic_state_transition(self.DT, self.ORDER, self.DIM), 200: constant_turn_rate_matrix(tr1, self.DT), 300: kinematic_state_transition(self.DT, self.ORDER, self.DIM), 400: constant_turn_rate_matrix(tr2, self.DT), 500: kinematic_state_transition(self.DT, self.ORDER, self.DIM) } p_noises = { 0: filterpy.common.Q_discrete_white_noise(self.DIM, self.DT, var=var, block_size=self.ORDER + 1) } target = DefinedTargetProcess(x0, st_models, p_noises, self.MAX_STEPS, self.ORDER, self.DIM) tracker = IMMEstimator(filters, probs, M) radar = PositionRadar(target, sn0, pfa, beamwidth, self.DIM, self.ORDER, angle_accuracy=theta_accuracy) computer = TrackingComputer(tracker, radar, n_max, P0=P0) super().__init__(computer)
def gen_env(): trajectory = Track() states = trajectory.gen_landing() x0 = states[0, 0] y0 = states[0, 3] z0 = states[0, 6] radar = Radar(x=0, y=2000) radar_filter_cv = RadarFilterCV(dim_x=9, dim_z=3, q=1., x0=x0, y0=y0, z0=z0, radar=radar) radar_filter_ca = RadarFilterCA(dim_x=9, dim_z=3, q=400., x0=x0, y0=y0, z0=z0, radar=radar) radar_filter_ct = RadarFilterCT(dim_x=9, dim_z=3, q=350., x0=x0, y0=y0, z0=z0, radar=radar) filters = [radar_filter_cv, radar_filter_ca, radar_filter_ct] mu = [0.33, 0.33, 0.33] trans = np.array([[0.998, 0.001, 0.001], [0.050, 0.900, 0.050], [0.001, 0.001, 0.998]]) imm = IMMEstimator(filters, mu, trans) benchmark_imm3 = Benchmark(radars=radar, radar_filter=imm, states=states) benchmark_imm3.launch_benchmark(with_nees=True)
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])
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((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())
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())
radars = [radar1, radar2] radar_filter_cv = MultipleRadarsFilterCV(dim_x=9, dim_z=6, q=1., radars=radars, x0=x0, y0=y0, z0=z0) radar_filter_ca = MultipleRadarsFilterCA(dim_x=9, dim_z=6, q=400., radars=radars, x0=x0, y0=y0, z0=z0) radar_filter_ct = MultipleRadarsFilterCT(dim_x=9, dim_z=6, q=350., radars=radars, x0=x0, y0=y0, z0=z0) filters = [radar_filter_cv, radar_filter_ca, radar_filter_ct] mu = [0.33, 0.33, 0.33] trans = np.array([[0.998, 0.001, 0.001], [0.050, 0.900, 0.050], [0.001, 0.001, 0.998]]) imm = IMMEstimator(filters, mu, trans) benchmark_imm3 = Benchmark(radars=radars, radar_filter=imm, states=states) benchmark_imm3.launch_benchmark(with_nees=True)
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()
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)
from filterpy.common import kinematic_kf from filterpy.kalman import ExtendedKalmanFilter import cv2 import time import filterpy as fp from filterpy.kalman import IMMEstimator np.set_printoptions(suppress=True) order = 1 kf1 = kinematic_kf(dim=2, order=order) kf2 = ExtendedKalmanFilter(4, 2) # do some settings of x, R, P etc. here, I'll just use the defaults kf2.Q *= 0 # no prediction error in second filter filters = [kf1, kf2] mu = [0.5, 0.5] # each filter is equally likely at the start trans = np.array([[0.97, 0.03], [0.03, 0.97]]) imm = IMMEstimator(filters, mu, trans) bg_image = cv2.imread('bg.png') # 500,1000 cv2.imshow('KalmanFilterDemo', bg_image) cv2.moveWindow("KalmanFilterDemo", 0, 0) window_ul = (8, 30) # x,y boxhsize = (20, 10) 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()