diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt
index 7313ab69a9..6ad44fedd3 100644
--- a/src/systemcmds/tests/CMakeLists.txt
+++ b/src/systemcmds/tests/CMakeLists.txt
@@ -99,6 +99,7 @@ px4_add_module(
 		-Wno-unused-variable
 		-Wno-vla-larger-than
 		-Wno-unused-but-set-parameter
+		-O2 # optimization needed to keep matrix code size reasonable
 	SRCS
 		${srcs}
 	DEPENDS
diff --git a/src/systemcmds/tests/test_microbench_matrix.cpp b/src/systemcmds/tests/test_microbench_matrix.cpp
index edf1459325..deec4e0233 100644
--- a/src/systemcmds/tests/test_microbench_matrix.cpp
+++ b/src/systemcmds/tests/test_microbench_matrix.cpp
@@ -95,7 +95,10 @@ public:
 
 private:
 
-	bool time_px4_matrix();
+	bool time_matrix_euler();
+	bool time_matrix_quaternion();
+	bool time_matrix_dcm();
+	bool time_matrix_pseduo_inverse();
 
 	void reset();
 
@@ -109,7 +112,10 @@ private:
 
 bool MicroBenchMatrix::run_tests()
 {
-	ut_run_test(time_px4_matrix);
+	ut_run_test(time_matrix_euler);
+	ut_run_test(time_matrix_quaternion);
+	ut_run_test(time_matrix_dcm);
+	ut_run_test(time_matrix_pseduo_inverse);
 
 	return (_tests_failed == 0);
 }
@@ -139,26 +145,36 @@ void MicroBenchMatrix::reset()
 			B16_4(j, i) = random(-10.0, 10.0);
 		}
 	}
+}
 
+bool MicroBenchMatrix::time_matrix_euler()
+{
+	PERF("matrix Euler from Quaternion", e = q, 100);
+	PERF("matrix Euler from Dcm", e = d, 100);
+	return true;
+}
+
+bool MicroBenchMatrix::time_matrix_quaternion()
+{
+	PERF("matrix Quaternion from Euler", q = e, 100);
+	PERF("matrix Quaternion from Dcm", q = d, 100);
+	return true;
+}
+
+bool MicroBenchMatrix::time_matrix_dcm()
+{
+	PERF("matrix Dcm from Euler", d = e, 100);
+	PERF("matrix Dcm from Quaternion", d = q, 100);
+	return true;
+}
+
+bool MicroBenchMatrix::time_matrix_pseduo_inverse()
+{
+	PERF("matrix 6x16 pseudo inverse (all non-zero columns)", A16 = matrix::geninv(B16), 100);
+	PERF("matrix 6x16 pseudo inverse (4 non-zero columns)", A16 = matrix::geninv(B16_4), 100);
+	return true;
 }
 
 ut_declare_test_c(test_microbench_matrix, MicroBenchMatrix)
 
-bool MicroBenchMatrix::time_px4_matrix()
-{
-	PERF("matrix Euler from Quaternion", e = q, 100);
-	PERF("matrix Euler from Dcm", e = d, 100);
-
-	PERF("matrix Quaternion from Euler", q = e, 100);
-	PERF("matrix Quaternion from Dcm", q = d, 100);
-
-	PERF("matrix Dcm from Euler", d = e, 100);
-	PERF("matrix Dcm from Quaternion", d = q, 100);
-
-	PERF("matrix 6x16 pseudo inverse (all non-zero columns)", A16 = matrix::geninv(B16), 100);
-	PERF("matrix 6x16 pseudo inverse (4 non-zero columns)", A16 = matrix::geninv(B16_4), 100);
-
-	return true;
-}
-
 } // namespace MicroBenchMatrix