mean_init = np.array([0, 0, 0, 0, 0])
cov_init = np.diag([1000, 1000, 30, 30, 0.1])**2 # THIS WILL NOT BE GOOD
mode_probabilities_init = np.array([p10, (1 - p10)])
mode_states_init = GaussParams(mean_init, cov_init)
init_imm_state = MixtureParameters(mode_probabilities_init,
[mode_states_init] * 2)
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))
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 = []
# 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)