diff --git a/matrix/SquareMatrix.hpp b/matrix/SquareMatrix.hpp
index 72b493e2a3..6b7da78eee 100644
--- a/matrix/SquareMatrix.hpp
+++ b/matrix/SquareMatrix.hpp
@@ -239,9 +239,9 @@ bool inv(const SquareMatrix<Type, M> & A, SquareMatrix<Type, M> & inv)
     }
 
     //check sanity of results
-    for (uint8_t i = 0; i < M; i++) {
-        for (uint8_t j = 0; j < M; j++) {
-            if (!PX4_ISFINITE(P(i,j))) {
+    for (size_t i = 0; i < M; i++) {
+        for (size_t j = 0; j < M; j++) {
+            if (!isfinite(P(i,j))) {
                 return false;
             }
         }
diff --git a/matrix/helper_functions.hpp b/matrix/helper_functions.hpp
index d39b672d75..bfd5b35f11 100644
--- a/matrix/helper_functions.hpp
+++ b/matrix/helper_functions.hpp
@@ -13,14 +13,19 @@
 namespace matrix
 {
 
+template<typename Type>
+bool isfinite(Type x) {
+#if defined (__PX4_NUTTX) || defined (__PX4_QURT)
+    return PX4_ISFINITE(x);
+#else
+    return std::isfinite(x);
+#endif
+}
+
 template<typename Type>
 Type wrap_pi(Type x)
 {
-#if defined (__PX4_NUTTX) || defined (__PX4_QURT)
     if (!isfinite(x)) {
-#else
-    if (!std::isfinite(x)) {
-#endif
         return x;
     }