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 x_bar_init = np.array([7100, 3620, 0, 0, 0]) P_bar_init = np.diag([40, 40, 10, 10, 0.1])**2
ax.set_title('Data') # show turn rate fig2, ax2 = plt.subplots(num=2, clear=True) ax2.plot(Xgt.T[4]) ax2.set_xlabel('time step') ax2.set_ylabel('turn rate') # %% a: tune by hand and comment # set parameters sigma_a = 1 sigma_z = 3 # create the model and estimator object dynmod = dynamicmodels.WhitenoiseAccelleration(sigma_a) measmod = measurmentmodels.CartesianPosition(sigma_z) ekf_filter = ekf.EKF(dynmod, measmod) print(ekf_filter) # make use of the @dataclass automatic repr # initialize mean and covariance x_bar_init = np.array([0, 0, 1, 1]).T # ??? P_bar_init = np.square(np.diag([75, 75, 10, 10])) init_ekfstate = ekf.GaussParams(x_bar_init, P_bar_init) # estimate ekfpred_list, ekfupd_list = ekf_filter.estimate_sequence(Z, init_ekfstate, Ts) # get statistics: stats = ekf_filter.performance_stats_sequence(K, Z=Z,
mode_probabilities_init = np.array([p10, p20]) 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)
sigma_a_CV = 0.22 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)
cov_init = np.diag([10, 10, 20, 20, 0.1]) ** 2 mode_probabilities_init1 = np.array([p10, (1 - p10)]) mode_states_init = GaussParams(mean_init, cov_init) init_imm_state1 = MixtureParameters(mode_probabilities_init1, [mode_states_init] * 2) mode_probabilities_init2 = np.array([0.34, 0.33, 0.33]) #arbitrary doesnt have much effect init_imm_state2 = MixtureParameters(mode_probabilities_init2, [mode_states_init] * 3) assert np.allclose( np.sum(mode_probabilities_init1), 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)) <<<<<<< Updated upstream dynamic_models.append(dynamicmodels.WhitenoiseAccelleration(sigma_a_CV_H, n=5)) ekf_filters = [] ekf_filters.append(ekf.EKF(dynamic_models[0], measurement_model)) ekf_filters.append(ekf.EKF(dynamic_models[1], measurement_model)) imm_filter1 = imm.IMM(ekf_filters, PI1) ekf_filters.append(ekf.EKF(dynamic_models[2], measurement_model)) imm_filter2 = imm.IMM(ekf_filters, PI2) tracker1 = pda.PDA(imm_filter1, clutter_intensity, PD, gate_size) tracker2 = pda.PDA(imm_filter2, clutter_intensity, PD, gate_size) ======= dynamic_models.append(dynamicmodels.WhitenoiseAccelleration(sigma_a_CV_HIGH, n=5)) #ADDED AV NADIA!!!
assert np.allclose(np.sum(PI, axis=1), 1), "rows of PI must sum to 1" mean_init = np.array([7116, 3617, 0, 0, 0]) # Sverre: er omtrent der sporet begynner. cov_init = np.diag([14, 14, 2, 2, 0.01]) ** 2 mode_probabilities_init = np.array([0.7, 0.1, 0.2]) #sverre: utvidet pga den tredje moden mode_states_init = GaussParams(mean_init, cov_init) init_imm_state = MixtureParameters(mode_probabilities_init, [mode_states_init] * 3) #sverre: må ganges med tre og ikke to pga. den tredje moden 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_high, n=5)) #five states: two for position, two for velocity and one for angle velocity 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)) 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.T[:2]) # %% tune single filters sigma_z = 8.7 sigma_a_CT = 0.2 sigma_a_CV = 5.5 sigma_omega = 5e-6 * np.pi init_state = { "mean": np.array(Xgt[0, :]), "cov": np.diag([1, 1, 1, 1, 0.005])**2 } init_state = GaussParams(init_state["mean"], init_state["cov"]) measurement_model = measurementmodels.CartesianPosition( sigma_z, state_dim=5) # note the 5 CV = dynamicmodels.WhitenoiseAccelleration(sigma_a_CV, n=5) # note the 5 CT = dynamicmodels.ConstantTurnrate(sigma_a_CT, sigma_omega) # make models filters = [] filters.append(ekf.EKF(CV, measurement_model)) filters.append(ekf.EKF( CT, measurement_model)) #, so you understand what is going on here pred = [] upd = [] stats = [] for ekf_filter in filters: ekfpred_list, ekfupd_list = ekf_filter.estimate_sequence(Z, init_state, Ts) stats.append( ekf_filter.performance_stats_sequence(