diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 5c73f37391..f2617d9da6 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -4,6 +4,8 @@
 
 #if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
 
+#pragma GCC optimize("O3")
+
 /*
   optionally turn down optimisation for debugging
  */