# Apply control on vehicle
for q in range(len(vehicle.other_vehicles)):
vehicle.other_vehicles[q].apply_control(
vehicle.other_agents[q].run_step())
vehicle.vehicle.apply_control(vehicle.agent.run_step())
vehicle.world.tick()
# Plotting ####################################
# rotate VO points from VO frame to robot start frame
trajectory_vo_np = (np.asarray(trajectory_vo) @ vo_compensation).T
trajectory_gt_np = np.asarray(trajectory_gt).T
trajectory_vio_np = vio_ekf.getTrajectory().T
ax.plot3D(
trajectory_gt_np[0],
trajectory_gt_np[1],
trajectory_gt_np[2],
"red",
label="Ground Truth",
)
ax.plot3D(
trajectory_vo_np[0],
trajectory_vo_np[1],
trajectory_vo_np[2],
"green",
label="Estimated (VO)",
