diff --git a/src/lib/matrix/matrix/AxisAngle.hpp b/src/lib/matrix/matrix/AxisAngle.hpp
index 049e54d277..01d5f517ab 100644
--- a/src/lib/matrix/matrix/AxisAngle.hpp
+++ b/src/lib/matrix/matrix/AxisAngle.hpp
@@ -6,19 +6,16 @@
 
 #pragma once
 
-#include "math.hpp"
+#include "Vector3.hpp"
 
 namespace matrix
 {
 
-template <typename Type>
-class Dcm;
-
 template <typename Type>
 class Euler;
 
-template <typename Type>
-class AxisAngle;
+template<typename Type>
+class Quaternion;
 
 /**
  * AxisAngle class
diff --git a/src/lib/matrix/matrix/Dcm.hpp b/src/lib/matrix/matrix/Dcm.hpp
index b640572097..a96ad85068 100644
--- a/src/lib/matrix/matrix/Dcm.hpp
+++ b/src/lib/matrix/matrix/Dcm.hpp
@@ -15,19 +15,20 @@
 
 #pragma once
 
-#include "math.hpp"
+#include "SquareMatrix.hpp"
+#include "Vector3.hpp"
 
 namespace matrix
 {
 
 template<typename Type>
-class Quaternion;
+class AxisAngle;
 
 template<typename Type>
 class Euler;
 
 template<typename Type>
-class AxisAngle;
+class Quaternion;
 
 /**
  * Direction cosine matrix class
diff --git a/src/lib/matrix/matrix/Dcm2.hpp b/src/lib/matrix/matrix/Dcm2.hpp
index 97aee7a622..d1682643ee 100644
--- a/src/lib/matrix/matrix/Dcm2.hpp
+++ b/src/lib/matrix/matrix/Dcm2.hpp
@@ -48,7 +48,8 @@
 
 #pragma once
 
-#include "math.hpp"
+#include "SquareMatrix.hpp"
+#include "Vector2.hpp"
 
 namespace matrix
 {
diff --git a/src/lib/matrix/matrix/Dual.hpp b/src/lib/matrix/matrix/Dual.hpp
index fad1581623..e7eafa0331 100644
--- a/src/lib/matrix/matrix/Dual.hpp
+++ b/src/lib/matrix/matrix/Dual.hpp
@@ -15,14 +15,12 @@
 
 #include <cmath>
 
-#include "math.hpp"
+#include "Scalar.hpp"
+#include "Vector.hpp"
 
 namespace matrix
 {
 
-template <typename Type, size_t M>
-class Vector;
-
 template <typename Scalar, size_t N>
 struct Dual {
 	static constexpr size_t WIDTH = N;
diff --git a/src/lib/matrix/matrix/Euler.hpp b/src/lib/matrix/matrix/Euler.hpp
index d885b85215..5a0bbdceda 100644
--- a/src/lib/matrix/matrix/Euler.hpp
+++ b/src/lib/matrix/matrix/Euler.hpp
@@ -15,17 +15,9 @@
 
 #pragma once
 
-#include "math.hpp"
-
 namespace matrix
 {
 
-template <typename Type>
-class Dcm;
-
-template <typename Type>
-class Quaternion;
-
 /**
  * Euler angles class
  *
diff --git a/src/lib/matrix/matrix/LeastSquaresSolver.hpp b/src/lib/matrix/matrix/LeastSquaresSolver.hpp
index 036b845ec4..80aed7cfee 100644
--- a/src/lib/matrix/matrix/LeastSquaresSolver.hpp
+++ b/src/lib/matrix/matrix/LeastSquaresSolver.hpp
@@ -14,7 +14,7 @@
 
 #pragma once
 
-#include "math.hpp"
+#include "Vector.hpp"
 
 namespace matrix
 {
diff --git a/src/lib/matrix/matrix/Matrix.hpp b/src/lib/matrix/matrix/Matrix.hpp
index 36654be608..b56c96c379 100644
--- a/src/lib/matrix/matrix/Matrix.hpp
+++ b/src/lib/matrix/matrix/Matrix.hpp
@@ -12,20 +12,12 @@
 #include <cstdio>
 #include <cstring>
 
-#include "math.hpp"
+#include "helper_functions.hpp"
+#include "Slice.hpp"
 
 namespace matrix
 {
 
-template <typename Type, size_t M>
-class Vector;
-
-template<typename Type, size_t M, size_t N>
-class Matrix;
-
-template <typename Type, size_t P, size_t Q, size_t M, size_t N>
-class Slice;
-
 template<typename Type, size_t M, size_t N>
 class Matrix
 {
diff --git a/src/lib/matrix/matrix/PseudoInverse.hpp b/src/lib/matrix/matrix/PseudoInverse.hpp
index 37a084b351..70404aed71 100644
--- a/src/lib/matrix/matrix/PseudoInverse.hpp
+++ b/src/lib/matrix/matrix/PseudoInverse.hpp
@@ -9,7 +9,8 @@
 
 #pragma once
 
-#include "math.hpp"
+#include "SquareMatrix.hpp"
+#include "Vector.hpp"
 
 namespace matrix
 {
diff --git a/src/lib/matrix/matrix/Quaternion.hpp b/src/lib/matrix/matrix/Quaternion.hpp
index 7155bdff1a..a2637606fd 100644
--- a/src/lib/matrix/matrix/Quaternion.hpp
+++ b/src/lib/matrix/matrix/Quaternion.hpp
@@ -28,21 +28,20 @@
 
 #pragma once
 
-#include "math.hpp"
+#include <float.h> // FLT_EPSILON
+
+#include "Vector3.hpp"
+#include "Vector4.hpp"
 
 namespace matrix
 {
 
-template <typename Type>
-class Dcm;
-
 template <typename Type>
 class Euler;
 
 template <typename Type>
 class AxisAngle;
 
-
 /**
  * Quaternion class
  *
diff --git a/src/lib/matrix/matrix/Scalar.hpp b/src/lib/matrix/matrix/Scalar.hpp
index f34551fbd4..c40b32912b 100644
--- a/src/lib/matrix/matrix/Scalar.hpp
+++ b/src/lib/matrix/matrix/Scalar.hpp
@@ -8,7 +8,7 @@
 
 #pragma once
 
-#include "math.hpp"
+#include "Matrix.hpp"
 
 namespace matrix
 {
diff --git a/src/lib/matrix/matrix/Slice.hpp b/src/lib/matrix/matrix/Slice.hpp
index da995a420b..3a7b51c485 100644
--- a/src/lib/matrix/matrix/Slice.hpp
+++ b/src/lib/matrix/matrix/Slice.hpp
@@ -8,8 +8,9 @@
 
 #pragma once
 
-#include "math.hpp"
-
+#include <cassert>
+#include <cstdio>
+#include <cmath>
 
 namespace matrix
 {
diff --git a/src/lib/matrix/matrix/SparseVector.hpp b/src/lib/matrix/matrix/SparseVector.hpp
index 40e790dfbf..79fc360db6 100644
--- a/src/lib/matrix/matrix/SparseVector.hpp
+++ b/src/lib/matrix/matrix/SparseVector.hpp
@@ -10,7 +10,7 @@
 
 #pragma once
 
-#include "math.hpp"
+#include "Vector.hpp"
 
 namespace matrix
 {
diff --git a/src/lib/matrix/matrix/SquareMatrix.hpp b/src/lib/matrix/matrix/SquareMatrix.hpp
index ce3b374992..e2e2084ae3 100644
--- a/src/lib/matrix/matrix/SquareMatrix.hpp
+++ b/src/lib/matrix/matrix/SquareMatrix.hpp
@@ -10,20 +10,11 @@
 
 #include <float.h> // FLT_EPSILON
 
-#include "math.hpp"
+#include "Slice.hpp"
 
 namespace matrix
 {
 
-template <typename Type, size_t M, size_t N>
-class Matrix;
-
-template <typename Type, size_t M>
-class Vector;
-
-template <typename Type, size_t P, size_t Q, size_t M, size_t N>
-class Slice;
-
 template <typename Type, size_t  M>
 class SquareMatrix : public Matrix<Type, M, M>
 {
diff --git a/src/lib/matrix/matrix/Vector.hpp b/src/lib/matrix/matrix/Vector.hpp
index dc80e28d2c..87b98afa71 100644
--- a/src/lib/matrix/matrix/Vector.hpp
+++ b/src/lib/matrix/matrix/Vector.hpp
@@ -8,14 +8,11 @@
 
 #pragma once
 
-#include "math.hpp"
+#include "Matrix.hpp"
 
 namespace matrix
 {
 
-template <typename Type, size_t M, size_t N>
-class Matrix;
-
 template<typename Type, size_t M>
 class Vector : public Matrix<Type, M, 1>
 {
diff --git a/src/lib/matrix/matrix/Vector2.hpp b/src/lib/matrix/matrix/Vector2.hpp
index 35ceb7c1ac..27b9ee90fd 100644
--- a/src/lib/matrix/matrix/Vector2.hpp
+++ b/src/lib/matrix/matrix/Vector2.hpp
@@ -8,14 +8,11 @@
 
 #pragma once
 
-#include "math.hpp"
+#include "Vector.hpp"
 
 namespace matrix
 {
 
-template <typename Type, size_t M>
-class Vector;
-
 template<typename Type>
 class Vector2 : public Vector<Type, 2>
 {
diff --git a/src/lib/matrix/matrix/Vector3.hpp b/src/lib/matrix/matrix/Vector3.hpp
index b67ab39a8b..b7247145c9 100644
--- a/src/lib/matrix/matrix/Vector3.hpp
+++ b/src/lib/matrix/matrix/Vector3.hpp
@@ -8,23 +8,14 @@
 
 #pragma once
 
-#include "math.hpp"
+#include "Vector.hpp"
 
 namespace matrix
 {
 
-template <typename Type, size_t M, size_t N>
-class Matrix;
-
-template <typename Type, size_t M>
-class Vector;
-
 template <typename Type>
 class Dcm;
 
-template <typename Type>
-class Vector2;
-
 template<typename Type>
 class Vector3 : public Vector<Type, 3>
 {
diff --git a/src/lib/matrix/matrix/Vector4.hpp b/src/lib/matrix/matrix/Vector4.hpp
index 088399847d..4eba1de639 100644
--- a/src/lib/matrix/matrix/Vector4.hpp
+++ b/src/lib/matrix/matrix/Vector4.hpp
@@ -41,17 +41,11 @@
 
 #pragma once
 
-#include "math.hpp"
+#include "Vector.hpp"
 
 namespace matrix
 {
 
-template <typename Type, size_t M, size_t N>
-class Matrix;
-
-template <typename Type, size_t M>
-class Vector;
-
 template<typename Type>
 class Vector4 : public Vector<Type, 4>
 {
diff --git a/src/lib/matrix/matrix/filter.hpp b/src/lib/matrix/matrix/filter.hpp
index 76c091d44a..983af10eab 100644
--- a/src/lib/matrix/matrix/filter.hpp
+++ b/src/lib/matrix/matrix/filter.hpp
@@ -1,6 +1,7 @@
 #pragma once
 
-#include "math.hpp"
+#include "Scalar.hpp"
+#include "SquareMatrix.hpp"
 
 namespace matrix
 {
diff --git a/src/lib/matrix/matrix/helper_functions.hpp b/src/lib/matrix/matrix/helper_functions.hpp
index 08e74d7115..d482a92cf2 100644
--- a/src/lib/matrix/matrix/helper_functions.hpp
+++ b/src/lib/matrix/matrix/helper_functions.hpp
@@ -2,7 +2,7 @@
 
 #include <cmath>
 
-#include "math.hpp"
+#include <px4_platform_common/defines.h>
 
 namespace matrix
 {
diff --git a/src/lib/matrix/matrix/integration.hpp b/src/lib/matrix/matrix/integration.hpp
index 338958ed4b..9ca190333e 100644
--- a/src/lib/matrix/matrix/integration.hpp
+++ b/src/lib/matrix/matrix/integration.hpp
@@ -1,6 +1,6 @@
 #pragma once
 
-#include "math.hpp"
+#include "Vector.hpp"
 
 namespace matrix
 {
diff --git a/src/lib/matrix/matrix/math.hpp b/src/lib/matrix/matrix/math.hpp
index dd80723d85..7f55520544 100644
--- a/src/lib/matrix/matrix/math.hpp
+++ b/src/lib/matrix/matrix/math.hpp
@@ -1,24 +1,20 @@
 #pragma once
 
-#include <assert.h>
-#include <px4_platform_common/defines.h>
-
+#include "AxisAngle.hpp"
+#include "Dcm.hpp"
+#include "Dcm2.hpp"
+#include "Dual.hpp"
+#include "Euler.hpp"
 #include "helper_functions.hpp"
-
+#include "LeastSquaresSolver.hpp"
 #include "Matrix.hpp"
-#include "SquareMatrix.hpp"
+#include "PseudoInverse.hpp"
+#include "Quaternion.hpp"
+#include "Scalar.hpp"
 #include "Slice.hpp"
+#include "SparseVector.hpp"
+#include "SquareMatrix.hpp"
 #include "Vector.hpp"
 #include "Vector2.hpp"
 #include "Vector3.hpp"
 #include "Vector4.hpp"
-#include "Euler.hpp"
-#include "Dcm.hpp"
-#include "Dcm2.hpp"
-#include "Scalar.hpp"
-#include "Quaternion.hpp"
-#include "AxisAngle.hpp"
-#include "LeastSquaresSolver.hpp"
-#include "Dual.hpp"
-#include "PseudoInverse.hpp"
-#include "SparseVector.hpp"
diff --git a/src/lib/matrix/test/MatrixDcm2Test.cpp b/src/lib/matrix/test/MatrixDcm2Test.cpp
index 32cdc36085..4e1b696ae6 100644
--- a/src/lib/matrix/test/MatrixDcm2Test.cpp
+++ b/src/lib/matrix/test/MatrixDcm2Test.cpp
@@ -34,6 +34,7 @@
 #include <gtest/gtest.h>
 #include <matrix/Dcm.hpp>
 #include <matrix/Dcm2.hpp>
+#include <matrix/Euler.hpp>
 
 using namespace matrix;
 
diff --git a/src/lib/matrix/test/MatrixFilterTest.cpp b/src/lib/matrix/test/MatrixFilterTest.cpp
index 10d1b786ae..a5104c7daa 100644
--- a/src/lib/matrix/test/MatrixFilterTest.cpp
+++ b/src/lib/matrix/test/MatrixFilterTest.cpp
@@ -33,6 +33,7 @@
 
 #include <gtest/gtest.h>
 #include <matrix/filter.hpp>
+#include <matrix/Vector.hpp>
 
 using namespace matrix;