Changed sampling the obstacle trajectory to j=i-1:i+1