initial_pos = H_global_to_start @ np.append(t_right_handed, 1)
trajectory_gt = [initial_pos]
# EKF Initialization #TODO #########################################################
# Convert the right-handed rotation to a quaternion, roll it to get the form w,x,y,z from x,y,z,w
initial_quat_wxyz = np.roll(r_right_handed.as_quat(), 1)
# initial_quat_wxyz = np.roll(R.from_matrix(car_start_r).as_quat(), 1)
## Initialize the EKF system #TODO check initial values
# vio_ekf = EKF(np.array([0,0,0]), np.array([0, 0, 0]), initial_quat_wxyz, debug=False)
vio_ekf = EKF(np.array([0, 0, 0]),
np.array([0, 0, 0]),
np.array([1, 0, 0, 0]),
debug=False)
vio_ekf.use_new_data = True
vio_ekf.setSigmaAccel(1.)
vio_ekf.setSigmaGyro(0.5)
vio_ekf.setSigmaVO(100.)
accel_list = []
gyro_list = []
# Main Loop #############################################################
max_length = 500
first = True
step = 0
while True:
step += 1
accel_list.append(accel)
gyro_list.append(gyro)
# vehicle.vehicle.apply_control(vehicle.agent.run_step())
carla_rotation_to_numpy_rotation_matrix(initial_rotation))
t_right_handed = np.array(
[initial_location.x, initial_location.y * -1, initial_location.z]).T
## Convert the right-handed rotation to a quaternion, roll it to get the form w,x,y,z from x,y,z,w
initial_quat = np.roll(r_right_handed.as_quat(), 1)
## Initialize the EKF system #TODO check initial values
vio_ekf = EKF(np.array([0, 0, 0]),
np.array([0, 0, 0]),
initial_quat,
debug=False)
vio_ekf.use_new_data = False
# --- Set instrument noise parameters
vio_ekf.setSigmaAccel(0.0)
vio_ekf.setSigmaGyro(0.0)
### Location transform ###############################################
H_local_to_global = np.eye(4)
H_local_to_global[:3, :3] = r_right_handed.as_matrix()
H_local_to_global[:3, 3] = t_right_handed
H_global_to_local = np.linalg.inv(H_local_to_global)
r_right_handed = R.from_matrix(H_global_to_local[:3, :3])
t_right_handed = H_global_to_local[:3, 3]
r_right_handed = r_right_handed.as_rotvec()
r_left_handed = -1 * r_right_handed * 180 / np.pi
t_left_handed = t_right_handed.T
t_left_handed[1] = t_left_handed[1] * -1
