diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5fae908d9a..909683df32 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,7 +10,11 @@ set(CMAKE_CXX_STANDARD 11)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
 if (NOT CMAKE_BUILD_TYPE)
-    set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "Build type" FORCE)
+    if(TESTING)
+        set(CMAKE_BUILD_TYPE "Debug" CACHE STRING "Build type" FORCE)
+    else()
+        set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "Build type" FORCE)
+    endif()
     message(STATUS "set build type to ${CMAKE_BUILD_TYPE}")
 endif()
 
diff --git a/matrix/Matrix.hpp b/matrix/Matrix.hpp
index c3b47ce839..188b5be619 100644
--- a/matrix/Matrix.hpp
+++ b/matrix/Matrix.hpp
@@ -96,11 +96,21 @@ public:
 
     inline Type operator()(size_t i, size_t j) const
     {
+        assert(i >= 0);
+        assert(i < M);
+        assert(j >= 0);
+        assert(j < N);
+
         return _data[i][j];
     }
 
     inline Type &operator()(size_t i, size_t j)
     {
+        assert(i >= 0);
+        assert(i < M);
+        assert(j >= 0);
+        assert(j < N);
+
         return _data[i][j];
     }
 
@@ -470,6 +480,11 @@ public:
 
     inline void swapRows(size_t a, size_t b)
     {
+        assert(a >= 0);
+        assert(a < M);
+        assert(b >= 0);
+        assert(b < M);
+
         if (a == b) {
             return;
         }
@@ -485,6 +500,11 @@ public:
 
     inline void swapCols(size_t a, size_t b)
     {
+        assert(a >= 0);
+        assert(a < N);
+        assert(b >= 0);
+        assert(b < N);
+
         if (a == b) {
             return;
         }
diff --git a/matrix/Slice.hpp b/matrix/Slice.hpp
index 21b2a4f09a..f61a009054 100644
--- a/matrix/Slice.hpp
+++ b/matrix/Slice.hpp
@@ -28,15 +28,30 @@ public:
         _data(const_cast<Matrix<Type, M, N>*>(data)) {
         static_assert(P <= M, "Slice rows bigger than backing matrix");
         static_assert(Q <= N, "Slice cols bigger than backing matrix");
+        assert(x0 >= 0);
+        assert(x0 + P <= M);
+        assert(y0 >= 0);
+        assert(y0 + Q <= N);
     }
 
     Type operator()(size_t i, size_t j) const
     {
+        assert(i >= 0);
+        assert(i < P);
+        assert(j >= 0);
+        assert(j < Q);
+
         return (*_data)(_x0 + i, _y0 + j);
     }
 
     Type &operator()(size_t i, size_t j)
+
     {
+        assert(i >= 0);
+        assert(i < P);
+        assert(j >= 0);
+        assert(j < Q);
+
         return (*_data)(_x0 + i, _y0 + j);
     }
 
@@ -97,23 +112,23 @@ public:
         return Slice<Type, R, S, M, N>(x0 + _x0, y0 + _y0, _data);
     }
 
-    void copyTo(Type dst[M*N]) const
+    void copyTo(Type dst[P*Q]) const
     {
         const Slice<Type, P, Q, M, N> &self = *this;
 
-        for (size_t i = 0; i < M; i++) {
-            for (size_t j = 0; j < N; j++) {
+        for (size_t i = 0; i < P; i++) {
+            for (size_t j = 0; j < Q; j++) {
                 dst[i*N+j] = self(i, j);
             }
         }
     }
 
-    void copyToColumnMajor(Type dst[M*N]) const
+    void copyToColumnMajor(Type dst[P*Q]) const
     {
         const Slice<Type, P, Q, M, N> &self = *this;
 
-        for (size_t i = 0; i < M; i++) {
-            for (size_t j = 0; j < N; j++) {
+        for (size_t i = 0; i < P; i++) {
+            for (size_t j = 0; j < Q; j++) {
                 dst[i+(j*M)] = self(i, j);
             }
         }
diff --git a/matrix/SquareMatrix.hpp b/matrix/SquareMatrix.hpp
index fcb225d324..d343bcfbfb 100644
--- a/matrix/SquareMatrix.hpp
+++ b/matrix/SquareMatrix.hpp
@@ -135,6 +135,10 @@ public:
     template <size_t Width>
     void uncorrelateCovariance(size_t first)
     {
+        static_assert(Width <= M, "Width bigger than matrix");
+        assert(first >= 0);
+        assert(first + Width <= M);
+
         SquareMatrix<Type, M> &self = *this;
         Vector<Type, Width> diag_elements = self.slice<Width, Width>(first, first).diag();
         self.uncorrelateCovarianceSetVariance(first, diag_elements);
@@ -143,10 +147,14 @@ public:
     template <size_t Width>
     void uncorrelateCovarianceSetVariance(size_t first, const Vector<Type, Width> &vec)
     {
+        static_assert(Width <= M, "Width bigger than matrix");
+        assert(first >= 0);
+        assert(first + Width <= M);
+
         SquareMatrix<Type, M> &self = *this;
         // zero rows and columns
-        self.slice<Width, M>(first, 0) = 0;
-        self.slice<M, Width>(0, first) = 0;
+        self.slice<Width, M>(first, 0) = Type(0);
+        self.slice<M, Width>(0, first) = Type(0);
 
         // set diagonals
         unsigned vec_idx = 0;
@@ -159,10 +167,14 @@ public:
     template <size_t Width>
     void uncorrelateCovarianceSetVariance(size_t first, Type val)
     {
+        static_assert(Width <= M, "Width bigger than matrix");
+        assert(first >= 0);
+        assert(first + Width <= M);
+
         SquareMatrix<Type, M> &self = *this;
         // zero rows and columns
-        self.slice<Width, M>(first, 0) = 0;
-        self.slice<M, Width>(0, first) = 0;
+        self.slice<Width, M>(first, 0) = Type(0);
+        self.slice<M, Width>(0, first) = Type(0);
 
         // set diagonals
         for (size_t idx = first; idx < first+Width; idx++) {
@@ -174,11 +186,15 @@ public:
     template <size_t Width>
     void makeBlockSymmetric(size_t first)
     {
+        static_assert(Width <= M, "Width bigger than matrix");
+        assert(first >= 0);
+        assert(first + Width <= M);
+
         SquareMatrix<Type, M> &self = *this;
         if(Width>1) {
             for (size_t row_idx = first+1; row_idx < first+Width; row_idx++) {
                 for (size_t col_idx = first; col_idx < row_idx; col_idx++) {
-                    Type tmp = self(row_idx,col_idx) + (self(col_idx,row_idx) - self(row_idx,col_idx)) / 2;
+                    Type tmp = self(row_idx,col_idx) + (self(col_idx,row_idx) - self(row_idx,col_idx)) / Type(2);
                     self(row_idx,col_idx) = tmp;
                     self(col_idx,row_idx) = tmp;
                 }
@@ -190,11 +206,15 @@ public:
     template <size_t Width>
     void makeRowColSymmetric(size_t first)
     {
+        static_assert(Width <= M, "Width bigger than matrix");
+        assert(first >= 0);
+        assert(first + Width <= M);
+
         SquareMatrix<Type, M> &self = *this;
         self.makeBlockSymmetric<Width>(first);
         for (size_t row_idx = first; row_idx < first+Width; row_idx++) {
             for (size_t col_idx = 0; col_idx < first; col_idx++) {
-                Type tmp = self(row_idx,col_idx) + (self(col_idx,row_idx) - self(row_idx,col_idx)) / 2;
+                Type tmp = self(row_idx,col_idx) + (self(col_idx,row_idx) - self(row_idx,col_idx)) / Type(2);
                 self(row_idx,col_idx) = tmp;
                 self(col_idx,row_idx) = tmp;
             }
@@ -210,6 +230,10 @@ public:
     template <size_t Width>
     bool isBlockSymmetric(size_t first, const Type eps = 1e-8f)
     {
+        static_assert(Width <= M, "Width bigger than matrix");
+        assert(first >= 0);
+        assert(first + Width <= M);
+
         SquareMatrix<Type, M> &self = *this;
         if(Width>1) {
             for (size_t row_idx = first+1; row_idx < first+Width; row_idx++) {
@@ -227,6 +251,10 @@ public:
     template <size_t Width>
     bool isRowColSymmetric(size_t first, const Type eps = 1e-8f)
     {
+        static_assert(Width <= M, "Width bigger than matrix");
+        assert(first >= 0);
+        assert(first + Width <= M);
+
         SquareMatrix<Type, M> &self = *this;
         for (size_t row_idx = first; row_idx < first+Width; row_idx++) {
             for (size_t col_idx = 0; col_idx < first; col_idx++) {
diff --git a/matrix/Vector.hpp b/matrix/Vector.hpp
index fe83505af9..40803ba8cc 100644
--- a/matrix/Vector.hpp
+++ b/matrix/Vector.hpp
@@ -42,12 +42,18 @@ public:
 
     inline Type operator()(size_t i) const
     {
+        assert(i >= 0);
+        assert(i < M);
+
         const MatrixM1 &v = *this;
         return v(i, 0);
     }
 
     inline Type &operator()(size_t i)
     {
+        assert(i >= 0);
+        assert(i < M);
+
         MatrixM1 &v = *this;
         return v(i, 0);
     }
diff --git a/matrix/math.hpp b/matrix/math.hpp
index 1bb9d67243..edfc1591fc 100644
--- a/matrix/math.hpp
+++ b/matrix/math.hpp
@@ -1,5 +1,6 @@
 #pragma once
 
+#include <assert.h>
 #include "stdlib_imports.hpp"
 #ifdef __PX4_QURT
 #include "dspal_math.h"