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,
