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}")
