diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
index 01376848d6..65fb4161cc 100644
--- a/libraries/AC_WPNav/AC_WPNav.cpp
+++ b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -386,7 +386,7 @@ void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f
     _flags.reached_destination = false;
 
     // initialise the limited speed to current speed along the track
-    Vector3f curr_vel = _inav->get_velocity();
+    const Vector3f &curr_vel = _inav->get_velocity();
     // get speed along track (note: we convert vertical speed into horizontal speed equivalent)
     float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z;
     _limited_speed_xy_cms = constrain_float(speed_along_track,0,_wp_speed_cms);
@@ -437,7 +437,7 @@ void AC_WPNav::advance_target_along_track(float dt)
     }
 
     // get current velocity
-    Vector3f curr_vel = _inav->get_velocity();
+    const Vector3f &curr_vel = _inav->get_velocity();
     // get speed along track
     float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z;
 
@@ -619,7 +619,7 @@ void AC_WPNav::get_loiter_position_to_velocity(float dt, float max_speed_cms)
 ///    converts desired velocities in lat/lon directions to accelerations in lat/lon frame
 void AC_WPNav::get_loiter_velocity_to_acceleration(float vel_lat, float vel_lon, float dt)
 {
-    Vector3f vel_curr = _inav->get_velocity();  // current velocity in cm/s
+    const Vector3f &vel_curr = _inav->get_velocity();  // current velocity in cm/s
     Vector3f vel_error;                         // The velocity error in cm/s.
     float accel_total;                          // total acceleration in cm/s/s