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 #init_state = tracker.init_filter_state({"mean": x_bar_init, "cov": P_bar_init}) init_state = GaussParams(x_bar_init, P_bar_init)
# 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, ekfpred_list=ekfpred_list, ekfupd_list=ekfupd_list,
[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) NEESpos = np.zeros(K) NEESvel = np.zeros(K)
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) print("lower bound: ", scipy.stats.distributions.chi2.ppf(0.05,K*Z.ndim)) print("upper bound: ", scipy.stats.distributions.chi2.ppf(0.95,K*Z.ndim)) # per filter
#dynamic model sigma_a_CT = 3.5 sigma_omega = 0.05 #Fører bruk av bare ett filter med CT modellen til dårlig filter consistency? mean_init = np.array([7116, 3617, 0, 0, 0]) cov_init = np.diag([14, 14, 2, 2, 0.01])**2 ekf_init = GaussParams(mean_init, cov_init) # make model measurement_model = measurementmodels.CartesianPosition(sigma_z, state_dim=5) if model == "CV": ekf_filter = ekf.EKF( dynamicmodels.WhitenoiseAccelleration(sigma_a_CV, n=5), measurement_model) elif model == "CT": ekf_filter = ekf.EKF( dynamicmodels.ConstantTurnrate(sigma_a_CT, sigma_omega), measurement_model) tracker = pda.PDA(ekf_filter, clutter_intensity, PD, gate_size) NEES = np.zeros(K) NEESpos = np.zeros(K) NEESvel = np.zeros(K) tracker_update = ekf_init tracker_update_list = [] tracker_predict_list = []
sigma_z = 1 sigma_a_CT = 1 sigma_omega = 1 # TODO init_state = {"mean": [None], "cov": np.diag([None]) ** 2} 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(None) # TODO, so you understand what is going on here pred = [] upd = [] stats = [] for ekf_filter in filters: ekfpred_list, ekfupd_list = # TODO stats.append( ekf_filter.performance_stats_sequence( K=K, Z=Z, ekfpred_list=ekfpred_list, ekfupd_list=ekfupd_list, X_true=Xgt, NEES_idx=ekf_filter.dynamic_model._all_idx, # HACK?
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( K=K, Z=Z, ekfpred_list=ekfpred_list, ekfupd_list=ekfupd_list, X_true=Xgt,