diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp
index 6d7b384c77..b2c4e97b4b 100644
--- a/libraries/Filter/HarmonicNotchFilter.cpp
+++ b/libraries/Filter/HarmonicNotchFilter.cpp
@@ -13,6 +13,11 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#ifndef HAL_DEBUG_BUILD
+#define AP_INLINE_VECTOR_OPS
+#pragma GCC optimize("O2")
+#endif
+
 #include "HarmonicNotchFilter.h"
 #include <GCS_MAVLink/GCS.h>
 
diff --git a/libraries/Filter/LowPassFilter.cpp b/libraries/Filter/LowPassFilter.cpp
index 246dc4cc02..c19322c759 100644
--- a/libraries/Filter/LowPassFilter.cpp
+++ b/libraries/Filter/LowPassFilter.cpp
@@ -4,7 +4,10 @@
 ///         the downside being that it's a little slower as it internally uses a float
 ///         and it consumes an extra 4 bytes of memory to hold the constant gain
 
-
+#ifndef HAL_DEBUG_BUILD
+#define AP_INLINE_VECTOR_OPS
+#pragma GCC optimize("O2")
+#endif
 #include "LowPassFilter.h"
 
 ////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/libraries/Filter/LowPassFilter2p.cpp b/libraries/Filter/LowPassFilter2p.cpp
index 93f9bb86ef..8154ab002d 100644
--- a/libraries/Filter/LowPassFilter2p.cpp
+++ b/libraries/Filter/LowPassFilter2p.cpp
@@ -1,3 +1,7 @@
+#ifndef HAL_DEBUG_BUILD
+#define AP_INLINE_VECTOR_OPS
+#pragma GCC optimize("O2")
+#endif
 #include "LowPassFilter2p.h"
 
 
diff --git a/libraries/Filter/NotchFilter.cpp b/libraries/Filter/NotchFilter.cpp
index ada79666b8..fb586d17db 100644
--- a/libraries/Filter/NotchFilter.cpp
+++ b/libraries/Filter/NotchFilter.cpp
@@ -13,6 +13,11 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#ifndef HAL_DEBUG_BUILD
+#define AP_INLINE_VECTOR_OPS
+#pragma GCC optimize("O2")
+#endif
+
 #include "NotchFilter.h"
 
 /*