def validate_ekf_transition_update(show_plot=True): # Load pickle validation_run = load_pickle(EKF_PICKLE) ground_truth_states = validation_run['states'] u = validation_run['controls'] dt = validation_run['t'][1:] - validation_run['t'][:-1] T = validation_run['t'].shape[0] # Initialize EKF localization ekf_loc = EkfLocalization(ground_truth_states[0], NoiseParams['Sigma0'], NoiseParams['R'], params.MapParams, validation_run['tf_base_to_camera'], NoiseParams['g']) # Simulate controls open_loop_states = np.zeros((T, ekf_loc.x.shape[0])) open_loop_states[0] = ekf_loc.x for i in range(T - 1): ekf_loc.transition_update(u[i], dt[i]) open_loop_states[i+1] = ekf_loc.x # Plot plt.clf() plt.plot(ground_truth_states[:,0], ground_truth_states[:,1], label="ground truth", color="black") plt.plot(open_loop_states[:,0], open_loop_states[:,1], label="open loop", color="green") plt.axis("equal") if show_plot: plt.legend(loc=0) plt.savefig("ekf_open_loop.png") print("Plot saved to ekf_open_loop.png")
def validate_localization_transition_model(): # Load pickle validation_run = load_pickle(EKF_PICKLE) u = validation_run['controls'] T = validation_run['t'].shape[0] validation = validation_run['transition_model_validation'] # Initialize EKF localization ekf_loc = EkfLocalization(validation_run['states'][0], NoiseParams['Sigma0'], NoiseParams['R'], params.MapParams, validation_run['tf_base_to_camera'], NoiseParams['g']) # Simulate controls for i in range(T): g, Gx, Gu = ekf_loc.transition_model(u[i], 0.1) g_ref, Gx_ref, Gu_ref = validation[i] if np.linalg.norm(g - g_ref) + np.linalg.norm(Gx - Gx_ref) + np.linalg.norm(Gu - Gu_ref) > 1e-2: print("At state x = {0} with u = {1} and dt = {2} got EkfLocalization.transition_model output:\n".format(ekf_loc.x, u, 0.1)) print(g) print(Gx) print(Gu) print("\nvs. the expected values\n") print(g_ref) print(Gx_ref) print(Gu_ref) return False print("EkfLocalization.transition_model() seems to be correct") return True
def validate_localization_compute_predicted_measurements(): # Load pickle validation_run = load_pickle(EKF_PICKLE) validation = validation_run['compute_predicted_measurements_validation'] # Initialize EKF localization ekf_loc = EkfLocalization(validation_run['states'][0], NoiseParams['Sigma0'], NoiseParams['R'], params.MapParams, validation_run['tf_base_to_camera'], NoiseParams['g']) # Compare measurements hs, Hs = ekf_loc.compute_predicted_measurements() for j in range(ekf_loc.map_lines.shape[1]): h, Hx = hs[:,j], Hs[j] h_ref, Hx_ref = validation[j] if np.linalg.norm(h - h_ref) + np.linalg.norm(Hx - Hx_ref) > 1e-3: print("At state x = {0} with m = {1} got EkfLocalization.compute_predicted_measurements:\n".format(ekf_loc.x, ekf_loc.map_lines[:,j])) print(h) print(Hx) print("\nvs. the expected values\n") print(h_ref) print(Hx_ref) return False print("EkfLocalization.compute_predicted_measurements() seems to be correct") return True
def validate_localization_compute_innovations(): # Test transition_model() and predicted_measurements() first if not validate_localization_transition_model() or \ not validate_localization_compute_predicted_measurements(): print("Validation of compute_innovations cannot proceed.") return False # Load pickle validation_run = load_pickle(EKF_PICKLE) validation_input = validation_run['compute_innovations_validation_input'] validation = validation_run['compute_innovations_validation'] scans = validation_run['scans'] T_scans = validation_run['t_scans'].shape[0] # Initialize EKF localization ekf_loc = EkfLocalization(validation_run['states'][0], NoiseParams['Sigma0'], NoiseParams['R'], params.MapParams, validation_run['tf_base_to_camera'], NoiseParams['g']) for i in range(T_scans): ekf_loc.x, ekf_loc.Sigma = validation_input[i] alpha, r, Q_raw, _, _ = ExtractLines(scans[0,i,:], scans[1,i,:], LineExtractionParams, NoiseParams['var_theta'], NoiseParams['var_rho']) z_raw = np.vstack((alpha, r)) v_list, R_list, H_list = ekf_loc.compute_innovations(z_raw, Q_raw) v_list_ref, R_list_ref, H_list_ref = validation[i] if len(v_list) != len(v_list_ref) or len(R_list) != len(R_list_ref) or len(H_list) != len(H_list_ref): print("You may have an error in EkfLocalization.compute_innovations.") print("v", v_list, v_list_ref) print("R", R_list, R_list_ref) print("H", H_list, H_list_ref) return False permutation = [np.argmin([np.linalg.norm(R - R_ref) for R_ref in R_list_ref]) for R in R_list] v_error = sum([np.linalg.norm(v_list[j] - v_list_ref[k]) for j, k in enumerate(permutation)]) R_error = sum([np.linalg.norm(R_list[j] - R_list_ref[k]) for j, k in enumerate(permutation)]) H_error = sum([np.linalg.norm(H_list[j] - H_list_ref[k]) for j, k in enumerate(permutation)]) if v_error + R_error + H_error > 1e-3: print("You may have an error in EkfLocalization.compute_innovations.") print("v", v_list, v_list_ref) print("R", R_list, R_list_ref) print("H", H_list, H_list_ref) return False print("EkfLocalization.compute_innovations() seems to be correct") return True
def validate_ekf_localization(show_plot=True): # Plot open loop validate_ekf_transition_update(False) # Load pickle validation_run = load_pickle(EKF_PICKLE) u = validation_run['controls'] t = validation_run['t'] T = t.shape[0] t_scans = validation_run['t_scans'] T_scans = t_scans.shape[0] scans = validation_run['scans'] # Initialize EKF localization ekf_loc = EkfLocalization(validation_run['states'][0], NoiseParams['Sigma0'], NoiseParams['R'], params.MapParams, validation_run['tf_base_to_camera'], NoiseParams['g']) # Iterate over states ekf_states = np.zeros((T, ekf_loc.x.shape[0])) ekf_states[0] = ekf_loc.x scan_states = np.zeros((T_scans, ekf_loc.x.shape[0])) scan_idx = 0 for i in range(T - 1): t1 = t[i+1] t0 = t[i] # Iterate over scans while scan_idx < T_scans and t_scans[scan_idx] < t1: # Transition update ekf_loc.transition_update(u[i], t_scans[scan_idx] - t0) t0 = t_scans[scan_idx] # Measurement update alpha, r, Q_raw, _, _ = ExtractLines(scans[0,scan_idx,:], scans[1,scan_idx,:], LineExtractionParams, NoiseParams['var_theta'], NoiseParams['var_rho']) z_raw = np.vstack((alpha, r)) ekf_loc.measurement_update(z_raw, Q_raw) scan_states[scan_idx] = ekf_loc.x scan_idx += 1 # Transition update ekf_loc.transition_update(u[i], t1 - t0) ekf_states[i+1] = ekf_loc.x # Plot plt.plot(ekf_states[:,0], ekf_states[:,1], label="EKF (known map)", color="red") plt.scatter(scan_states[:,0], scan_states[:,1], marker = "x", label="measurement update", color="blue") if show_plot: plt.legend(loc=0) plt.savefig("ekf_localization.png") print("Plot saved to ekf_localization.png")