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$")