diff --git a/libraries/AP_GPS/AP_GPS_MTK.cpp b/libraries/AP_GPS/AP_GPS_MTK.cpp
index 3ba943cd08..546426e8f5 100644
--- a/libraries/AP_GPS/AP_GPS_MTK.cpp
+++ b/libraries/AP_GPS/AP_GPS_MTK.cpp
@@ -37,6 +37,9 @@ AP_GPS_MTK::init(enum GPS_Engine_Setting nav_setting)
     // set WAAS on
     _port->print(WAAS_ON);
 
+    // Set Nav Threshold to 0 m/s
+    _port->print(MTK_NAVTHRES_OFF);
+
     // set initial epoch code
     _epoch = TIME_OF_DAY;
 
diff --git a/libraries/AP_GPS/AP_GPS_MTK16.cpp b/libraries/AP_GPS/AP_GPS_MTK16.cpp
index 61d304dd68..027b0408b3 100644
--- a/libraries/AP_GPS/AP_GPS_MTK16.cpp
+++ b/libraries/AP_GPS/AP_GPS_MTK16.cpp
@@ -44,6 +44,9 @@ AP_GPS_MTK16::init(enum GPS_Engine_Setting nav_setting)
     // set WAAS on
     _port->print(WAAS_ON);
 
+    // Set Nav Threshold to 0 m/s
+    _port->print(MTK_NAVTHRES_OFF);
+
     // set initial epoch code
     _epoch = TIME_OF_DAY;
     _time_offset = 0;
diff --git a/libraries/AP_GPS/AP_GPS_MTK_Common.h b/libraries/AP_GPS/AP_GPS_MTK_Common.h
index 06338600a1..e08ed98b82 100644
--- a/libraries/AP_GPS/AP_GPS_MTK_Common.h
+++ b/libraries/AP_GPS/AP_GPS_MTK_Common.h
@@ -24,10 +24,12 @@
 
 #define MTK_BAUD_RATE_38400 "$PMTK251,38400*27\r\n"
 
-#define SBAS_ON			"$PMTK313,1*2E\r\n"
-#define SBAS_OFF		"$PMTK313,0*2F\r\n"
+#define MTK_NAVTHRES_OFF "$PMTK397,0*23\r\n"  // Set Nav Threshold (the minimum speed the GPS must be moving to update the position) to 0 m/s
 
-#define WAAS_ON			"$PSRF151,1*3F\r\n"
-#define WAAS_OFF		"$PSRF151,0*3E\r\n"
+#define SBAS_ON	        "$PMTK313,1*2E\r\n"
+#define SBAS_OFF	"$PMTK313,0*2F\r\n"
+
+#define WAAS_ON         "$PMTK301,2*2E\r\n"
+#define WAAS_OFF        "$PMTK301,0*2C\r\n"
 
 #endif // AP_GPS_MTK_Common_h