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