plt.show(block=False) plt.pause(plotpause) # %% setup and track # Model parameters EKF sigma_a = 3 # From EKF exc 3 sigma_z = 10 # From EKF exc 3 # PDA relevant PD = 0.9 clutter_intensity = 10e-6 # TODO # Basically estimated from poisson clutter model, makes no sense to have fixed one gate_size = 3 CV = dynamicmodels.WhitenoiseAccelleration(sigma_a) measurement_model = measurementmodels.CartesianPosition(sigma_z) ekf_filter = ekf.EKF(CV, measurement_model) tracker = pda.PDA(ekf_filter, clutter_intensity, PD, gate_size) # initialize z_0 = Z[0] x_bar_init = np.array([*z_0[0], 0, 0]) P_bar_init = np.zeros((4, 4)) P_bar_init[[0, 1], [0, 1]] = 2 * sigma_z**2 P_bar_init[[2, 3], [2, 3]] = 5**2 init_state = tracker.init_filter_state({"mean": x_bar_init, "cov": P_bar_init}) # allocate
PI = np.array([[PI11, (1 - PI11)], [(1 - PI22), PI22]]) assert np.allclose(np.sum(PI, axis=1), 1), "rows of PI must sum to 1" 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)