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)
tracker_update = init_state
tracker_update_list = []
tracker_predict_list = []
tracker_estimate_list = []
# estimate
for k, (Zk, x_true_k, Tsk) in enumerate(zip(Z, Xgt, Ts)):
tracker_predict = tracker.predict(tracker_update, Tsk)
tracker_update = tracker.update(Zk, tracker_predict)
tracker_estimate = tracker.estimate(tracker_update)
NEES[k] = estats.NEES(*tracker_estimate, x_true_k, idxs=np.arange(4))
NEESpos[k] = estats.NEES(*tracker_estimate, x_true_k, idxs=np.arange(2))
NEESvel[k] = estats.NEES(*tracker_estimate, x_true_k, idxs=np.arange(2, 4))
tracker_predict_list.append(tracker_predict)
tracker_update_list.append(tracker_update)
tracker_estimate_list.append(tracker_estimate)
x_hat = np.array([est.mean for est in tracker_estimate_list])
prob_hat = np.array([upd.cov for upd in tracker_update_list])
# calculate a performance metrics
poserr = np.linalg.norm(x_hat[:, :2] - Xgt[:, :2], axis=0)
velerr = np.linalg.norm(x_hat[:, 2:4] - Xgt[:, 2:4], axis=0)
posRMSE = np.sqrt(np.mean(
poserr**2)) # not true RMSE (which is over monte carlo simulations)
P_bar_init[[0, 1], [0, 1]] = 2 * sigma_z**2
P_bar_init[[2, 3], [2, 3]] = 10**2
#init_state = tracker.init_filter_state({"mean": x_bar_init, "cov": P_bar_init})
init_state = GaussParams(x_bar_init, P_bar_init)
tracker_update = init_state
tracker_update_list = []
tracker_predict_list = []
# estimate
for k, (Zk, x_true_k) in enumerate(zip(Z, Xgt)):
Zk = Zk[0]
tracker_predict = tracker.predict(tracker_update, Ts)
tracker_update = tracker.update(Zk, tracker_predict)
x, P = tracker_update
NEES[k] = est.NEES(x, P, x_true_k, idxs=np.arange(4))
NEESpos[k] = est.NEES(x, P, x_true_k, idxs=np.arange(2))
NEESvel[k] = est.NEES(x, P, x_true_k, idxs=np.arange(2, 4))
#NEES[k] = tracker.state_filter.NEES(tracker_update, x_true_k, idx=np.arange(4))
#NEESpos[k] = tracker.state_filter.NEES(tracker_update, x_true_k, idx=np.arange(2))
#NEESvel[k] = tracker.state_filter.NEES(tracker_update, x_true_k, idx=np.arange(2, 4))
tracker_predict_list.append(tracker_predict)
tracker_update_list.append(tracker_update)
x_hat = np.array([upd.mean for upd in tracker_update_list])
# calculate a performance metric
posRMSE = np.sqrt(np.mean(np.sum((x_hat[:, :2] - Xgt[:, :2])**2, axis=1)))
# position RMSE
velRMSE = np.sqrt(np.mean(np.sum((x_hat[:, 2:4] - Xgt[:, 2:4])**2, axis=1)))
# velocity RMSE
# %% plots
fig3, ax3 = plt.subplots(num=3, clear=True)
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
init_state = tracker.init_filter_state({"mean": x_bar_init, "cov": P_bar_init})
tracker_update = init_state
tracker_update_list = []
tracker_predict_list = []
# estimate
for k, (Zk, x_true_k) in enumerate(zip(Z, Xgt)):
tracker_predict = tracker.predict(tracker_update, Ts)
tracker_update = tracker.update(Zk, tracker_predict)
NEES[k] = estimationstatistics.NEES(*tracker_update,
x_true_k,
idxs=np.arange(4))
NEESpos[k] = estimationstatistics.NEES(*tracker_update,
x_true_k,
idxs=np.arange(2))
NEESvel[k] = estimationstatistics.NEES(*tracker_update,
x_true_k,
idxs=np.arange(2, 4))
tracker_predict_list.append(tracker_predict)
tracker_update_list.append(tracker_update)
x_hat = np.array([upd.mean for upd in tracker_update_list])
# calculate a performance metric
posRMSE = np.sqrt(np.mean(np.sum((x_hat[:, :2] - Xgt[:, :2])**2, axis=1)))
velRMSE = np.sqrt(np.mean(np.sum((x_hat[:, 2:4] - Xgt[:, 2:4])**2, axis=1)))
ekfpred_list = []
ekfupd_list = []
ekfupd = init_ekfstate
NIS = np.empty(K)
NEES_pred = np.empty(K)
NEES_upd = np.empty(K)
dists_pred = np.empty((K, 2))
dists_upd = np.empty((K, 2))
# estimate
for k, (zk, x_true_k) in enumerate(zip(Z[2:], Xgt[2:])):
ekfpred = ekf_filter.predict(ekfupd, Ts)
ekfupd = ekf_filter.update(zk, ekfpred)
NIS[k] = ekf_filter.NIS(zk, ekfpred)
NEES_pred[k] = estats.NEES(*ekfpred, x_true_k)
NEES_upd[k] = estats.NEES(*ekfupd, x_true_k)
diff_pred = ekfpred.mean - x_true_k[:4]
diff_upd = ekfupd.mean - x_true_k[:4]
dists_pred[k] = np.linalg.norm(diff_pred[:2]), np.linalg.norm(
diff_pred[2:])
dists_upd[k] = np.linalg.norm(diff_upd[:2]), np.linalg.norm(diff_upd[2:])
ekfpred_list.append(ekfpred)
ekfupd_list.append(ekfupd)
x_bar = np.array([pred.mean for pred in ekfpred_list])
P_bar = np.array([pred.cov for pred in ekfpred_list])
x_hat = np.array([upd.mean for upd in ekfupd_list])
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
init_state = tracker.init_filter_state({"mean": x_bar_init, "cov": P_bar_init})
tracker_update = init_state
tracker_update_list = []
tracker_predict_list = []
# estimate
for k, (Zk, x_true_k) in enumerate(zip(Z, Xgt)):
tracker_predict = tracker.predict(tracker_update, Ts) # TODO
tracker_update = tracker.update(Zk, tracker_predict) # TODO
NEES[k] = eststat.NEES(*tracker_update, x_true_k,
idxs=np.arange(4)) # TODO
NEESpos[k] = eststat.NEES(*tracker_update, x_true_k,
idxs=np.arange(2)) # TODO
NEESvel[k] = eststat.NEES(*tracker_update, x_true_k,
idxs=np.arange(2, 4)) # TODO
tracker_predict_list.append(tracker_predict)
tracker_update_list.append(tracker_update)
x_hat = np.array([upd.mean for upd in tracker_update_list])
# calculate a performance metric
posRMSE = np.sqrt(np.mean(np.sum((x_hat[:, :2] - Xgt[:, :2])**2,
axis=1))) # TODO: position RMSE
velRMSE = np.sqrt(np.mean(np.sum((x_hat[:, 2:4] - Xgt[:, 2:4])**2,
axis=1))) # TODO: velocity RMSE
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]] = 20 ** 2
init_state = gaussparams.GaussParams(x_bar_init, P_bar_init)
tracker_update = init_state
tracker_update_list = []
tracker_predict_list = []
# estimate
for k, (Zk, x_true_k) in enumerate(zip(Z, Xgt)):
tracker_predict = tracker.predict(init_state, Ts)
tracker_update = tracker.update(Zk, filter_state=tracker_predict) #hva med sensor_state
updated_state = tracker_update.mean
updated_cov = tracker_update.cov
NEES[k] = estats.NEES(updated_state, updated_cov, x_true_k, idxs=np.arange(4))
#NEESpos[k] = estats.NEES(updated_state, updated_cov, x_true_k, idxs=np.arange(2))
#NEESvel[k] = estats.NEES(updated_state, updated_cov, x_true_k, idxs=np.arange(2, 4))
tracker_predict_list.append(tracker_predict)
tracker_update_list.append(tracker_update)
x_hat = np.array([upd.mean for upd in tracker_update_list])
# calculate a performance metric
posRMSE = np.sqrt(np.mean(np.sum((x_hat[:,:2]-Xgt[:,:2])**2,axis=1)))
velRMSE = np.sqrt(np.mean(np.sum((x_hat[:,2:4]-Xgt[:,2:4])**2,axis=1)))
# %% plots
fig3, ax3 = plt.subplots(num=3, clear=True)
ax3.plot(*x_hat.T[:2], label=r"$\hat x$")
ax3.plot(*Xgt.T[:2], label="$x$")
