mode_states_init = GaussParams(mean_init, cov_init) init_imm_state = MixtureParameters(mode_probabilities_init, [mode_states_init] * 2) if run_three_models: mode_probabilities_init = np.array([p10, p20, p30]) init_imm_state = MixtureParameters(mode_probabilities_init, [mode_states_init] * 3) assert np.allclose(np.sum(mode_probabilities_init), 1), "initial mode probabilities must sum to 1" # make model measurement_model = measurementmodels.CartesianPosition(sigma_z, state_dim=5) dynamic_models: List[dynamicmodels.DynamicModel] = [] dynamic_models.append(dynamicmodels.WhitenoiseAccelleration(sigma_a_CV, n=5)) dynamic_models.append(dynamicmodels.ConstantTurnrate(sigma_a_CT, sigma_omega)) ekf_filters = [] ekf_filters.append(ekf.EKF(dynamic_models[0], measurement_model)) ekf_filters.append(ekf.EKF(dynamic_models[1], measurement_model)) if run_three_models: dynamic_models.append( dynamicmodels.WhitenoiseAccelleration(sigma_a_CV_high, n=5)) ekf_filters.append(ekf.EKF(dynamic_models[2], measurement_model)) imm_filter = imm.IMM(ekf_filters, PI) tracker = pda.PDA(imm_filter, clutter_intensity, PD, gate_size) # init_imm_pda_state = tracker.init_filter_state(init__immstate) NEES = np.zeros(K) NEESpos = np.zeros(K)
ax1.scatter(*Z_plot_data.T, color="C1") ax1.plot(*Xgt.T[:2], color="C0", linewidth=1.5) ax1.set_title("True trajectory and the nearby measurements") plt.show(block=False) sigma_z = 20 sigma_omega = 0.1 PD = 0.65 clutter_intensity = 4 / (4000 * 4000) gate_size = 5 useTurnRateModel = True if useTurnRateModel: sigma_a = 4 # works really well with sigma_omega = 0.1 dynamic_model = dynamicmodels.ConstantTurnrate(sigma_a, sigma_omega) else: # constant velocity model sigma_a = 2 dynamic_model = dynamicmodels.WhitenoiseAccelleration(sigma_a, n=5) measurement_model = measurementmodels.CartesianPosition(sigma_z, state_dim=5) ekf_filter = ekf.EKF(dynamic_model, measurement_model) tracker = pda.PDA(ekf_filter, clutter_intensity, PD, gate_size) # allocate NEES = np.zeros(K) NEESpos = np.zeros(K) NEESvel = np.zeros(K) # initialize
sigma_a_CT = 0.07 sigma_omega = 0.0016 * np.pi # initial values init_mean = np.array([0, 0, 2, 0, 0]) init_cov = np.diag([25, 25, 3, 3, 0.0005]) ** 2 init_state_CV = GaussParams(init_mean[:4], init_cov[:4, :4]) # get rid of turn rate init_state_CT = GaussParams(init_mean, init_cov) # same init otherwise init_states = [init_state_CV, init_state_CT] # create models measurement_model_CV = measurementmodels.CartesianPosition(sigma_z) measurement_model_CT = measurementmodels.CartesianPosition(sigma_z, state_dim=5) CV = dynamicmodels.WhitenoiseAccelleration(sigma_a_CV) CT = dynamicmodels.ConstantTurnrate(sigma_a_CT, sigma_omega) # create filters filters = [] filters.append(ekf.EKF(CV, measurement_model_CV)) filters.append(ekf.EKF(CT, measurement_model_CT)) # allocate pred = [] upd = [] NIS = np.empty((2, K)) NEES_pred = np.empty((2, K)) NEES_upd = np.empty((2, K)) err_pred = np.empty((2, 2, K)) # (filters, vel/pos, time) err_upd = np.empty((2, 2, K)) # (filters, vel/pos, time)