Collision prediction

[EricPedley]

def collision_prediction(drone_positions: list[tuple[np.ndarray, np.ndarray], current_pos: np.ndarray, current_velocity: np.ndarray, next_wp: np.ndarray) -> tuple[time_to_collision: float, no_go_zone: np.ndarray):
'''
drone_position will have each tuple being (x, covariance) where x is [x,y,z,vx,vy,vz,radius] and covariance is a 7x7 matrix describing the covariance of those estimates.

current_pos, current_velocity and next_wp are of shape (3,)

no_go_zone should be of shape (n,3) as a list of 3d points describing a convex shape.
'''

[bdoget]