def main():
data_path = '/home/nadav/studies/mapping_and_perception_autonomous_robots/project_2/Ex3_verB'
result_dir = r'/home/nadav/studies/mapping_and_perception_autonomous_robots/project_2/results'
cur_date_time = time.strftime("%Y.%m.%d-%H.%M")
result_dir_timed = os.path.join(result_dir, f'{cur_date_time}')
print(f'saving to: {result_dir_timed}')
os.makedirs(result_dir_timed, exist_ok=True)
ekf_slam_func(data_path, result_dir_timed)
if SAVE_VIDEO:
create_video(result_dir_timed)
# dataset_number = '0005' # video
result_dir = r'/home/nadav/studies/mapping_and_perception_autonomous_robots/first_project/results'
cur_date_time = time.strftime("%Y.%m.%d-%H.%M")
start_frame = 0
skip_frames = 1 # gap between two frames
skip_velo = 20 # gap between two measurements
x_size_m = 500
y_size_m = 500
resolution_cell_m = 20 * 1e-2
occ_params = {
'p_hit': 0.7, # default 0.7
'p_miss': 0.4, # default 0.4
'occ_th': 0.8, # default 0.8
'free_th': 0.2
} # default 0.2
result_dir_timed = os.path.join(
result_dir,
f'{cur_date_time}_{dataset_number}_skipvelo_{skip_velo}_{occ_params}_ve_vn_icp'
)
print(f'saving to: {result_dir_timed}')
os.makedirs(result_dir_timed, exist_ok=True)
create_occupancy_map(basedir, date, dataset_number, x_size_m, y_size_m,
resolution_cell_m, skip_frames, skip_velo,
result_dir_timed, start_frame, occ_params)
create_video(result_dir_timed)
def kalman_filter(result_dir_timed, data):
""" init data """
car_w_coordinates_m, car_w_coordinates_lon_lat, _, _, _, delta_time, time_sec = exract_data_vectors(
data)
delta_x = car_w_coordinates_m[:, 0][1::] - car_w_coordinates_m[:, 0][:-1:]
delta_y = car_w_coordinates_m[:, 1][1::] - car_w_coordinates_m[:, 1][:-1:]
vx = np.divide(delta_x, delta_time[1::])
vy = np.divide(delta_y, delta_time[1::])
vxvy = np.array([vx, vy]).T
vxvy = np.vstack([np.array([0, 0]), vxvy])
''' plot GT LLA and ENU data '''
plt.figure()
plt.suptitle('GPS GT trajectory')
plt.subplot(1, 2, 1)
plt.scatter(car_w_coordinates_m[:, 0],
car_w_coordinates_m[:, 1],
color='blue',
s=1,
label='GT')
start_index = 50
plt.text(car_w_coordinates_m[start_index, 0],
car_w_coordinates_m[start_index, 1], 'start')
plt.text(car_w_coordinates_m[-start_index, 0],
car_w_coordinates_m[-start_index, 1], 'end')
plt.title('car coordinate ENU')
plt.xlabel('east [m]')
plt.ylabel('north [m]')
plt.grid()
plt.subplot(1, 2, 2)
plt.scatter(car_w_coordinates_lon_lat[:, 0],
car_w_coordinates_lon_lat[:, 1],
color='blue',
s=1,
label='GT')
plt.text(car_w_coordinates_lon_lat[start_index, 0],
car_w_coordinates_lon_lat[start_index, 1], 'start')
plt.text(car_w_coordinates_lon_lat[-start_index, 0],
car_w_coordinates_lon_lat[-start_index, 1], 'end')
plt.title('car coordinate LLA')
plt.grid()
plt.xlabel('lon [deg]')
plt.ylabel('lat [deg]')
plt.show(block=False)
''' add noise to GT data '''
sigma_noise = 3
noised_car_w_coordinates_m = car_w_coordinates_m + np.random.normal(
0, sigma_noise, car_w_coordinates_m.shape)
plt.figure()
plt.scatter(car_w_coordinates_m[:, 0],
car_w_coordinates_m[:, 1],
s=1,
color='blue',
label='GT')
plt.scatter(noised_car_w_coordinates_m[:, 0],
noised_car_w_coordinates_m[:, 1],
s=1,
marker='x',
color='red',
label='noised_gt')
plt.legend()
plt.grid()
plt.title('car coordinate ENU w/wo noise')
plt.xlabel('east [m]')
plt.ylabel('north [m]')
plt.show(block=False)
''' Kalman filter - constant velocity model '''
# initialization
x_0 = 0.
y_0 = 0.
v_x_0 = 0.
v_y_0 = 0.
sigma_0_x = 3.
sigma_0_y = 3.
sigma_0_vx = 3.
sigma_0_vy = 3.
sigma_a = 1.5
total_est_meu, dead_reckoning, total_est_sigma = kalman_constant_velocity(
delta_time, noised_car_w_coordinates_m, sigma_0_vx, sigma_0_vy,
sigma_0_x, sigma_0_y, sigma_a, x_0, y_0, v_x_0, v_y_0,
result_dir_timed, car_w_coordinates_m, vxvy)
if SAVE_VIDEO:
create_video(result_dir_timed)
# total_est_meu = kalman_constant_velocity_flip(delta_time, noised_car_w_coordinates_m, sigma_0_vx, sigma_0_vy,
# sigma_0_x,
# sigma_0_y, sigma_a, x_0, y_0, v_x_0, v_y_0)
max_E, rmse = get_maxe_rmse(car_w_coordinates_m, total_est_meu)
print(f'kalman rmse={round(rmse, 3)} :: maxE={round(max_E, 3)}')
max_E_dead, rmse_dead = get_maxe_rmse(
car_w_coordinates_m[-dead_reckoning.shape[0]:, :], dead_reckoning)
print(
f'dead_reckoning rmse={round(rmse_dead, 3)} :: maxE={round(max_E_dead, 3)}'
)
# rmse, total_est_meu = explore_kalman_cv(car_w_coordinates_m, delta_time, noised_car_w_coordinates_m)
plt.figure()
plt.title(
rf'Kalman constant velocity $\sigma_x = \sigma_y={sigma_noise}$ - RMSE={round(rmse, 2)} [m], maxE={round(max_E, 2)} [m]'
)
plt.scatter(car_w_coordinates_m[:, 0],
car_w_coordinates_m[:, 1],
s=1,
label='GT')
plt.scatter(noised_car_w_coordinates_m[:, 0],
noised_car_w_coordinates_m[:, 1],
s=1,
marker='x',
color='red',
label='noised_gt')
plt.scatter(total_est_meu[:, 0],
total_est_meu[:, 1],
s=1,
marker='x',
color='green',
label='kalman CV')
plt.xlabel('x [m]')
plt.ylabel('y [m]')
plt.grid()
plt.legend()
plt.show(block=False)
a = 3
diff_x = car_w_coordinates_m[:, 0] - total_est_meu[:, 0]
diff_y = car_w_coordinates_m[:, 1] - total_est_meu[:, 1]
plt.figure()
plt.subplot(2, 1, 1)
plt.plot(diff_x, label='error in x [m]')
plt.plot(np.sqrt(total_est_sigma[:, 0, 0]), color='red', label='sigma x')
plt.plot(-np.sqrt(total_est_sigma[:, 0, 0]), color='red', label='sigma x')
plt.title('Error in x [m] and variance x')
plt.xlabel('sample number')
plt.ylabel('x error [m] and x variance')
plt.grid()
plt.subplot(2, 1, 2)
plt.plot(diff_y, label='error in y [m]')
plt.plot(np.sqrt(total_est_sigma[:, 1, 1]), color='red', label='sigma y')
plt.plot(-np.sqrt(total_est_sigma[:, 1, 1]), color='red', label='sigma y')
plt.title('Error in y [m] and variance y')
plt.xlabel('sample number')
plt.ylabel('y error [m] and y variance')
plt.grid()
plt.show(block=False)
a = 3
def extended_kalman_filter(result_dir_timed, data):
""" init data """
car_w_coordinates_m, car_w_coordinates_lon_lat, car_yaw, car_vf, car_wz, delta_time, time_sec = exract_data_vectors(
data)
''' plot GT LLA and ENU data '''
plt.figure()
plt.suptitle('GPS GT trajectory')
plt.subplot(1, 2, 1)
plt.scatter(car_w_coordinates_m[:, 0], car_w_coordinates_m[:, 1], color='blue', s=1, label='GT')
start_index = 50
plt.text(car_w_coordinates_m[start_index, 0], car_w_coordinates_m[start_index, 1], 'start')
plt.text(car_w_coordinates_m[-start_index, 0], car_w_coordinates_m[-start_index, 1], 'end')
plt.title('car coordinate ENU')
plt.xlabel('east [m]')
plt.ylabel('north [m]')
plt.grid()
plt.subplot(1, 2, 2)
plt.scatter(car_w_coordinates_lon_lat[:, 0], car_w_coordinates_lon_lat[:, 1], color='blue', s=1, label='GT')
plt.text(car_w_coordinates_lon_lat[start_index, 0], car_w_coordinates_lon_lat[start_index, 1], 'start')
plt.text(car_w_coordinates_lon_lat[-start_index, 0], car_w_coordinates_lon_lat[-start_index, 1], 'end')
plt.title('car coordinate LLA')
plt.grid()
plt.xlabel('lon [deg]')
plt.ylabel('lat [deg]')
plt.show(block=False)
plt.figure()
plt.subplot(3, 1, 1)
plt.plot(time_sec, np.rad2deg(car_yaw))
plt.grid()
plt.xlabel('Time [sec]')
plt.ylabel('yaw [deg]')
plt.subplot(3, 1, 2)
plt.plot(time_sec, car_vf)
plt.grid()
plt.xlabel('Time [sec]')
plt.ylabel('vf [m/s]')
plt.subplot(3, 1, 3)
plt.plot(time_sec, car_wz)
plt.grid()
plt.xlabel('Time [sec]')
plt.ylabel('angular rate [rad/s]')
plt.show(block=False)
''' add noise to GT data '''
sigma_noise = 3
noised_car_w_coordinates_m = car_w_coordinates_m + np.random.normal(0, sigma_noise, car_w_coordinates_m.shape)
plt.figure()
plt.scatter(car_w_coordinates_m[:, 0], car_w_coordinates_m[:, 1], s=1, color='blue', label='GT')
plt.scatter(noised_car_w_coordinates_m[:, 0], noised_car_w_coordinates_m[:, 1], s=1, marker='x', color='red',
label='noised_gt')
plt.legend()
plt.grid()
plt.title('car coordinate ENU w/wo noise')
plt.xlabel('east [m]')
plt.ylabel('north [m]')
plt.show(block=False)
''' Extended Kalman filter - constant velocity model - same initial condition as Kalman'''
if False:
sigmot = {
'sigma_x': 3.,
'sigma_y': 3.,
'sigma_vx': 3.,
'sigma_vy': 3.,
'sigma_yaw_rate': 0.2,
'sigma_yaw_initial': np.pi}
meu_0 = np.array([noised_car_w_coordinates_m[0][0], noised_car_w_coordinates_m[0][1], car_yaw[0]])
sigma_0 = np.diag([sigmot['sigma_x'] ** 2, sigmot['sigma_y'] ** 2, sigmot['sigma_yaw_initial'] ** 2])
data_vec = {
'car_w_coordinates_m': noised_car_w_coordinates_m,
'yaw': car_yaw,
'vf': car_vf,
'wz': car_wz,
'delta_time': delta_time}
ekf = ExtendedKalmanFilter(meu_0, sigma_0)
total_est_meu = ekf.perform_ekf(data_vec, sigmot)
max_E, rmse = get_maxe_rmse(car_w_coordinates_m, total_est_meu)
plt.figure()
plt.scatter(car_w_coordinates_m[:, 0], car_w_coordinates_m[:, 1], s=1, color='blue', label='GT')
plt.scatter(noised_car_w_coordinates_m[:, 0], noised_car_w_coordinates_m[:, 1], s=1, marker='x', color='red',
label='noised_gt')
plt.scatter(total_est_meu[:, 0], total_est_meu[:, 1], s=1, marker='x', color='green', label='kalman CV')
plt.legend()
plt.grid()
plt.title(
fr'Extended Kalman - $\sigma_x=\sigma_y={sigmot["sigma_x"]}, \sigma_v={sigmot["sigma_vx"]}, \sigma_\omega={sigmot["sigma_yaw_rate"]}$ - RMSE={round(rmse, 2)} [m], maxE={round(max_E, 2)} [m]')
plt.xlabel('east [m]')
plt.ylabel('north [m]')
plt.show(block=False)
a = 3
''' add noise to IMU '''
wz_sigma_noise = 0.2
noised_wz = car_wz + np.random.normal(0, wz_sigma_noise, car_wz.shape)
vf_sigma_noise = 2
noised_car_vf = car_vf + np.random.normal(0, vf_sigma_noise, car_vf.shape)
plt.figure()
plt.subplot(2, 1, 1)
plt.scatter(time_sec, car_wz, s=1, label='gt yaw rate')
plt.scatter(time_sec, noised_wz, s=1, label='noised yaw rate')
plt.grid()
plt.xlabel('Time [sec]')
plt.ylabel('yaw rate [deg/sec]')
plt.subplot(2, 1, 2)
plt.scatter(time_sec, car_vf, s=1, label='gt vf')
plt.scatter(time_sec, noised_car_vf, s=1, label='noised vf')
plt.grid()
plt.xlabel('Time [sec]')
plt.ylabel('vf [m/s]')
plt.show(block=False)
''' EKF - with suitable initial condition '''
sigmot = {
'sigma_x': 3.,
'sigma_y': 3.,
'sigma_vx': 3.,
'sigma_vy': 3.,
'sigma_yaw_rate': 0.3,
'sigma_yaw_initial': np.pi}
meu_0 = np.array([noised_car_w_coordinates_m[0][0], noised_car_w_coordinates_m[0][1], car_yaw[0]])
sigma_0 = np.diag([sigmot['sigma_x'] ** 2, sigmot['sigma_y'] ** 2, sigmot['sigma_yaw_initial'] ** 2])
data_vec = {
'car_w_coordinates_m': noised_car_w_coordinates_m,
'yaw': car_yaw,
'vf': noised_car_vf,
'wz': noised_wz,
'delta_time': delta_time}
save_video_dict = {'car_w_coordinates_m': car_w_coordinates_m,
'noised_car_w_coordinates_m': noised_car_w_coordinates_m,
'result_dir_timed': result_dir_timed}
ekf = ExtendedKalmanFilter(meu_0, sigma_0)
total_est_meu, total_sigma = ekf.perform_ekf(data_vec, sigmot, save_video_dict)
if SAVE_VIDEO:
create_video(result_dir_timed)
max_E, rmse = get_maxe_rmse(car_w_coordinates_m, total_est_meu)
plt.figure()
plt.scatter(car_w_coordinates_m[:, 0], car_w_coordinates_m[:, 1], s=1, color='blue', label='GT')
plt.scatter(noised_car_w_coordinates_m[:, 0], noised_car_w_coordinates_m[:, 1], s=1, marker='x', color='red',
label='noised_gt')
plt.scatter(total_est_meu[:, 0], total_est_meu[:, 1], s=1, marker='x', color='green', label='kalman CV')
plt.legend()
plt.grid()
plt.title(
fr'Extended Kalman - $\sigma_x=\sigma_y={sigma_noise}, \sigma_v={vf_sigma_noise}, \sigma_\omega={wz_sigma_noise}$ - RMSE={round(rmse, 2)} [m], maxE={round(max_E, 2)} [m]')
plt.xlabel('east [m]')
plt.ylabel('north [m]')
plt.show(block=False)
a = 3
diff_x = car_w_coordinates_m[:, 0] - total_est_meu[:, 0]
diff_y = car_w_coordinates_m[:, 1] - total_est_meu[:, 1]
total_est_theta = total_est_meu[:, 2]
mod_car_yaw = [cur_yaw if ii < 975 else cur_yaw - 2*np.pi for ii, cur_yaw in enumerate(car_yaw)] # np.where(car_yaw<2, car_yaw, car_yaw-2*np.pi)
diff_theta = mod_car_yaw - total_est_theta
# diff_theta = np.where(diff_theta<np.pi/2, diff_theta, diff_theta-np.pi)
# diff_theta = np.deg2rad(np.mod(np.rad2deg(car_yaw) - np.rad2deg(total_est_meu[:, 2]),180))
# plt.figure()
# plt.plot(mod_car_yaw)
# plt.plot(total_est_theta)
# plt.plot(diff_theta)
# plt.show(block=False)
plt.figure(figsize=[15, 10])
plt.subplot(3, 1, 1)
plt.plot(diff_x, label='error in x [m]')
plt.plot(np.sqrt(total_sigma[:, 0, 0]), color='red', label='sigma x')
plt.plot(-np.sqrt(total_sigma[:, 0, 0]), color='red')
# plt.title('Error in x [m] and variance x')
plt.xlabel('sample number')
plt.ylabel('x error [m] and x variance')
plt.legend()
plt.grid()
plt.subplot(3, 1, 2)
plt.plot(diff_y, label='error in y [m]')
plt.plot(np.sqrt(total_sigma[:, 1, 1]), color='red', label='sigma y')
plt.plot(-np.sqrt(total_sigma[:, 1, 1]), color='red')
# plt.title('Error in y [m] and variance y')
plt.xlabel('sample number')
plt.ylabel('y error [m] and y variance')
plt.legend()
plt.grid()
plt.subplot(3, 1, 3)
plt.plot(diff_theta, label=r'error in $\theta$ [rad]')
plt.plot(np.sqrt(total_sigma[:, 2, 2]), color='red', label=r'sigma $\theta$')
plt.plot(-np.sqrt(total_sigma[:, 2, 2]), color='red')
# plt.title(r'Error in $\theta$ [rad] and variance $\theta$')
plt.xlabel('sample number')
plt.ylabel(r'$\theta$ error [rad] and $\theta$ variance')
plt.grid()
plt.legend()
plt.show(block=False)
a=3