# 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())
# continue
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(5.)
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())
