sh.set_offsets(Zk)
th.set_text(f"measurements at step {k}")
fig2.canvas.draw_idle()
plt.show(block=False)
plt.pause(plotpause)
# %%
sigma_a = 5# TODO
sigma_z = 5# TODO
PD = 0.5# TODO
clutter_intensity = 2# TODO
gate_size = 100# TODO
dynamic_model = dynamicmodels.WhitenoiseAccelleration(sigma_a)
measurement_model = measurementmodels.CartesianPosition(sigma_z)
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([*Z[0][true_association[0] - 1], 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]] = 10 ** 2
sigma_z = 20
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})
# show turnrate
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 = 2.6
sigma_z = 3.1
# create the model and estimator object
dynmod = dynamicmodels.WhitenoiseAccelleration(sigma_a)
measmod = measurementmodels.CartesianPosition(sigma_z)
ekf_filter = ekf.EKF(dynmod, measmod)
print(ekf_filter)
# Optimal init for model
mean = np.array([*Z[1], *(Z[1] - Z[0]) / Ts])
cov11 = sigma_z**2 * np.eye(2)
cov12 = sigma_z**2 * np.eye(2) / Ts
cov22 = (2 * sigma_z**2 / Ts**2 + sigma_a**2 * Ts / 3) * np.eye(2)
cov = np.block([[cov11, cov12], [cov12.T, cov22]])
init_ekfstate = GaussParams(mean, cov)
ekfpred_list = []
ekfupd_list = []
ekfupd = init_ekfstate
NIS = np.empty(K)
