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) NEESvel = np.zeros(K) tracker_update = init_imm_state tracker_update_list = [] tracker_predict_list = [] tracker_estimate_list = [] # estimate for k, (Zk, x_true_k, Tsk) in enumerate(zip(Z, Xgt, Ts)):
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!!! ekf_filters1 = [] ekf_filters1.append(ekf.EKF(dynamic_models[0], measurement_model)) ekf_filters1.append(ekf.EKF(dynamic_models[1], measurement_model)) imm_filter1 = imm.IMM(ekf_filters, PI) #CV, CT and CV_high PI2 = tunes... (?????)
# %% tune IMM by only looking at the measurements sigma_z = 3 sigma_a_CV = 0.2 sigma_a_CT = 0.1 sigma_omega = 0.002 * np.pi PI = np.array([[0.95, 0.05], [0.05, 0.95]]) assert np.allclose(PI.sum(axis=1), 1), "rows of PI must sum to 1" # make model measurement_model = measurementmodels.CartesianPosition(sigma_z, state_dim=5) CV = dynamicmodels.WhitenoiseAccelleration(sigma_a_CV, n=5) CT = dynamicmodels.ConstantTurnrate(sigma_a_CT, sigma_omega) ekf_filters: List[StateEstimator[GaussParams]] = [] ekf_filters.append(ekf.EKF(CV, measurement_model)) ekf_filters.append(ekf.EKF(CT, measurement_model)) imm_filter: imm.IMM[GaussParams] = imm.IMM(ekf_filters, PI) init_weights = np.array([0.5] * 2) init_mean = [0] * 5 init_cov = np.diag( [1] * 5 ) # HAVE TO BE DIFFERENT: use intuition, eg. diag guessed distance to true values squared. init_mode_states = [GaussParams(init_mean, init_cov)] * 2 # copy of the two modes init_immstate = MixtureParameters(init_weights, init_mode_states) imm_preds = [] imm_upds = [] imm_ests = [] updated_immstate = init_immstate for zk in Z: predicted_immstate = imm_filter.predict(updated_immstate, Ts)
measurement_model = measurementmodels.CartesianPosition(sigma_z, state_dim=5) CV = dynamicmodels.WhitenoiseAccelleration(sigma_a_CVlow, n=5) CVh = dynamicmodels.WhitenoiseAccelleration(sigma_a_CVhigh, n=5) CT = dynamicmodels.ConstantTurnrate(sigma_a_CT, sigma_omega) ekf_filters = [] ekf_filters.append(ekf.EKF(CV, measurement_model)) # ekf_filters.append(ekf.EKF(CVh, measurement_model)) ekf_filters.append(ekf.EKF(CT, measurement_model)) init_state = { "weight": np.array([0.1, 0.4, 0.3]), # Mode probs: optional "mean": Xgt[0, :], "cov": np.diag([1, 1, 1, 1, 0.005])**2, } imm_filter = imm.IMM(filters=ekf_filters, PI=PI) imm_preds, imm_upds, imm_ests = imm_filter.estimate_sequence(Z, init_state, Ts) # extract some data x_est = np.array([est.mean for est in imm_ests]) prob_est = np.array([upds.weights for upds in imm_upds]) # consistency NISes_comb = [imm_filter.NISes(zk, pred_k) for zk, pred_k in zip(Z, imm_preds)] NIS = np.array([n[0] for n in NISes_comb]) NISes = np.array([n[1] for n in NISes_comb]) ANIS = np.mean(NIS, axis=0) CINIS = np.array(scipy.stats.chi2.interval(confprob, df)) CIANIS = np.array(scipy.stats.chi2.interval(confprob, df * K)) / K print(f"ANIS={ANIS} with CIANIS={CIANIS}")