pub fn distance_to_goal(current: &[f64; 2], goal: &[f64; 2]) -> ([f64; 2], f64)
Computes the distance and error vector from current position to goal.