Fix off-track tracking in AutoLineSmoothVel (#13321)

* Use position instead of last setpoint

This calculates the target velocities better taking into account disturbances along
the flight route. Previously entry angles and more were calculated assuming the flight path
originates directly from the direction of the previous waypoint. This corrects this assumption
to instead make the direction come from the vehicle location.

* Allow to specify a final speed given a braking distance.

This is to allow planning to not stop at a waypoint, but instead
to reach the waypoint while maintaining a certain velocity

* Updated src/lib/matrix

* Account for speed at target when determining constraints

* Separate constraints into x/y components

* Use setpoint position, not vehicle position

* Fix whitespace, add documentation

src/lib/CollisionPrevention/CollisionPrevention.cpp

@@ -439,7 +439,7 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec
 						const float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance);
 						const float vel_max_posctrl = xy_p * stop_distance;
 
-						const float vel_max_smooth = math::trajectory::computeMaxSpeedFromBrakingDistance(max_jerk, max_accel, stop_distance);
+						const float vel_max_smooth = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_accel, stop_distance, 0.f);
 						const float projection = bin_direction.dot(setpoint_dir);
 						float vel_max_bin = vel_max;