# 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())