Merge pull request #3136 from dronecrew/matrix

Separate matrix lib into own repo, added testing and coverage

.gitmodules

@@ -25,3 +25,6 @@
 [submodule "unittests/googletest"]
 	path = unittests/googletest
 	url = https://github.com/google/googletest.git
+[submodule "src/lib/matrix"]
+	path = src/lib/matrix
+	url = https://github.com/dronecrew/matrix.git

CMakeLists.txt

@@ -232,6 +232,7 @@ px4_add_git_submodule(TARGET git_nuttx PATH "NuttX")
 px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal")
 px4_add_git_submodule(TARGET git_jmavsim PATH "Tools/jMAVSim")
 px4_add_git_submodule(TARGET git_gazebo PATH "Tools/sitl_gazebo")
+px4_add_git_submodule(TARGET git_matrix PATH "src/lib/matrix")
 
 add_custom_target(submodule_clean
 	WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}

cmake/common/px4_base.cmake

@@ -622,7 +622,7 @@ function(px4_add_common_flags)
 		)
 
 	list(APPEND added_include_dirs
-		src/lib/eigen
+		src/lib/matrix
 		)
 
 	set(added_link_dirs) # none used currently

cmake/ros-CMakeLists.txt

@@ -136,6 +136,7 @@ include_directories(
   ${CMAKE_BINARY_DIR}/src/modules
   src/
   src/lib
+  src/lib/matrix
   ${EIGEN_INCLUDE_DIRS}
   integrationtests
 )

src/lib/mathlib/math/Matrix.hpp

@@ -49,7 +49,7 @@
 #ifdef CONFIG_ARCH_ARM
 #include "../CMSIS/Include/arm_math.h"
 #else
-#include "modules/local_position_estimator/matrix/src/Matrix.hpp"
+#include "matrix/matrix.hpp"
 #endif
 #include <platforms/px4_defines.h>
 

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -811,7 +811,7 @@ void BlockLocalPositionEstimator::predict()
 	Q(X_bz, X_bz) = _pn_b_noise_power.get();
 
 	// continuous time kalman filter prediction
-	Matrix<float, n_x, 1> dx = (A * _x + B * _u) * getDt();
+	Vector<float, n_x> dx = (A * _x + B * _u) * getDt();
 
 	// only predict for components we have
 	// valid measurements for
@@ -904,16 +904,16 @@ void BlockLocalPositionEstimator::correctFlow()
 	_flowY += global_speed[1] * dt;
 
 	// measurement
-	Vector2f y;
+	Vector<float, 2> y;
 	y(0) = _flowX;
 	y(1) = _flowY;
 
 	// residual
-	Vector2f r = y - C * _x;
+	Vector<float, 2> r = y - C * _x;
 
 	// residual covariance, (inverse)
 	Matrix<float, n_y_flow, n_y_flow> S_I =
-		(C * _P * C.transpose() + R).inverse();
+		inv<float, n_y_flow>(C * _P * C.transpose() + R);
 
 	// fault detection
 	float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -981,17 +981,17 @@ void BlockLocalPositionEstimator::correctSonar()
 	}
 
 	// measurement
-	Matrix<float, n_y_sonar, 1> y;
+	Vector<float, n_y_sonar> y;
 	y(0) = (d - _sonarAltHome) *
 	       cosf(_sub_att.get().roll) *
 	       cosf(_sub_att.get().pitch);
 
 	// residual
-	Matrix<float, n_y_sonar, 1> r = y - C * _x;
+	Vector<float, n_y_sonar> r = y - C * _x;
 
 	// residual covariance, (inverse)
 	Matrix<float, n_y_sonar, n_y_sonar> S_I =
-		(C * _P * C.transpose() + R).inverse();
+		inv<float, n_y_sonar>(C * _P * C.transpose() + R);
 
 	// fault detection
 	float beta = sqrtf((r.transpose()  * (S_I * r))(0, 0));
@@ -1032,7 +1032,7 @@ void BlockLocalPositionEstimator::correctSonar()
 void BlockLocalPositionEstimator::correctBaro()
 {
 
-	Matrix<float, n_y_baro, 1> y;
+	Vector<float, n_y_baro> y;
 	y(0) = _sub_sensor.get().baro_alt_meter[0] - _baroAltHome;
 
 	// baro measurement matrix
@@ -1046,8 +1046,8 @@ void BlockLocalPositionEstimator::correctBaro()
 
 	// residual
 	Matrix<float, n_y_baro, n_y_baro> S_I =
-		((C * _P * C.transpose()) + R).inverse();
+		inv<float, n_y_baro>((C * _P * C.transpose()) + R);
-	Matrix<float, n_y_baro, 1> r = y - (C * _x);
+	Vector<float, n_y_baro> r = y - (C * _x);
 
 	// fault detection
 	float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1060,7 +1060,7 @@ void BlockLocalPositionEstimator::correctBaro()
 		}
 
 		// lower baro trust
-		S_I = ((C * _P * C.transpose()) + R * 10).inverse();
+		S_I = inv<float, n_y_baro>((C * _P * C.transpose()) + R * 10);
 
 	} else if (_baroFault) {
 		_baroFault = FAULT_NONE;
@@ -1104,15 +1104,15 @@ void BlockLocalPositionEstimator::correctLidar()
 		R(0, 0) = cov;
 	}
 
-	Matrix<float, n_y_lidar, 1> y;
+	Vector<float, n_y_lidar> y;
 	y.setZero();
 	y(0) = (d - _lidarAltHome) *
 	       cosf(_sub_att.get().roll) *
 	       cosf(_sub_att.get().pitch);
 
 	// residual
-	Matrix<float, n_y_lidar, n_y_lidar> S_I = ((C * _P * C.transpose()) + R).inverse();
+	Matrix<float, n_y_lidar, n_y_lidar> S_I = inv<float, n_y_lidar>((C * _P * C.transpose()) + R);
-	Matrix<float, n_y_lidar, 1> r = y - C * _x;
+	Vector<float, n_y_lidar> r = y - C * _x;
 
 	// fault detection
 	float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1166,7 +1166,7 @@ void BlockLocalPositionEstimator::correctGps()  	// TODO : use another other met
 	//printf("home: lat %10g, lon, %10g alt %10g\n", _sub_home.lat, _sub_home.lon, double(_sub_home.alt));
 	//printf("local: x %10g y %10g z %10g\n", double(px), double(py), double(pz));
 
-	Matrix<float, 6, 1> y;
+	Vector<float, 6> y;
 	y.setZero();
 	y(0) = px;
 	y(1) = py;
@@ -1213,8 +1213,8 @@ void BlockLocalPositionEstimator::correctGps()  	// TODO : use another other met
 	R(5, 5) = var_vz;
 
 	// residual
-	Matrix<float, 6, 1> r = y - C * _x;
+	Vector<float, 6> r = y - C * _x;
-	Matrix<float, 6, 6> S_I = (C * _P * C.transpose() + R).inverse();
+	Matrix<float, 6, 6> S_I = inv<float, 6>(C * _P * C.transpose() + R);
 
 	// fault detection
 	float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1245,7 +1245,7 @@ void BlockLocalPositionEstimator::correctGps()  	// TODO : use another other met
 		}
 
 		// trust GPS less
-		S_I = ((C * _P * C.transpose()) + R * 10).inverse();
+		S_I = inv<float, 6>((C * _P * C.transpose()) + R * 10);
 
 	} else if (_gpsFault) {
 		_gpsFault = FAULT_NONE;
@@ -1266,7 +1266,7 @@ void BlockLocalPositionEstimator::correctGps()  	// TODO : use another other met
 void BlockLocalPositionEstimator::correctVision()
 {
 
-	Matrix<float, 3, 1> y;
+	Vector<float, 3> y;
 	y.setZero();
 	y(0) = _sub_vision_pos.get().x - _visionHome(0);
 	y(1) = _sub_vision_pos.get().y - _visionHome(1);
@@ -1287,7 +1287,7 @@ void BlockLocalPositionEstimator::correctVision()
 	R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get();
 
 	// residual
-	Matrix<float, n_y_vision, n_y_vision> S_I = ((C * _P * C.transpose()) + R).inverse();
+	Matrix<float, n_y_vision, n_y_vision> S_I = inv<float, n_y_vision>((C * _P * C.transpose()) + R);
 	Matrix<float, n_y_vision, 1> r = y - C * _x;
 
 	// fault detection
@@ -1301,7 +1301,7 @@ void BlockLocalPositionEstimator::correctVision()
 		}
 
 		// trust less
-		S_I = ((C * _P * C.transpose()) + R * 10).inverse();
+		S_I = inv<float, n_y_vision>((C * _P * C.transpose()) + R * 10);
 
 	} else if (_visionFault) {
 		_visionFault = FAULT_NONE;
@@ -1322,7 +1322,7 @@ void BlockLocalPositionEstimator::correctVision()
 void BlockLocalPositionEstimator::correctmocap()
 {
 
-	Matrix<float, n_y_mocap, 1> y;
+	Vector<float, n_y_mocap> y;
 	y.setZero();
 	y(Y_mocap_x) = _sub_mocap.get().x - _mocapHome(0);
 	y(Y_mocap_y) = _sub_mocap.get().y - _mocapHome(1);
@@ -1345,7 +1345,7 @@ void BlockLocalPositionEstimator::correctmocap()
 	R(Y_mocap_z, Y_mocap_z) = mocap_p_var;
 
 	// residual
-	Matrix<float, n_y_mocap, n_y_mocap> S_I = ((C * _P * C.transpose()) + R).inverse();
+	Matrix<float, n_y_mocap, n_y_mocap> S_I = inv<float, n_y_mocap>((C * _P * C.transpose()) + R);
 	Matrix<float, n_y_mocap, 1> r = y - C * _x;
 
 	// fault detection
@@ -1359,7 +1359,7 @@ void BlockLocalPositionEstimator::correctmocap()
 		}
 
 		// trust less
-		S_I = ((C * _P * C.transpose()) + R * 10).inverse();
+		S_I = inv<float, n_y_mocap>((C * _P * C.transpose()) + R * 10);
 
 	} else if (_mocapFault) {
 		_mocapFault = FAULT_NONE;

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -6,7 +6,7 @@
 #include <lib/geo/geo.h>
 
 #ifdef USE_MATRIX_LIB
-#include "matrix/src/Matrix.hpp"
+#include "matrix/Matrix.hpp"
 using namespace matrix;
 #else
 #include <Eigen/Eigen>
@@ -304,7 +304,7 @@ private:
 	perf_counter_t _err_perf;
 
 	// state space
-	Matrix<float, n_x, 1>  _x; // state vector
+	Vector<float, n_x>  _x; // state vector
-	Matrix<float, n_u, 1>  _u; // input vector
+	Vector<float, n_u>  _u; // input vector
 	Matrix<float, n_x, n_x>  _P; // state covariance matrix
 };

src/modules/local_position_estimator/CMakeLists.txt

@@ -34,8 +34,6 @@ if(${OS} STREQUAL "nuttx")
 	list(APPEND MODULE_CFLAGS -Wframe-larger-than=6500)
 elseif(${OS} STREQUAL "posix")
 	list(APPEND MODULE_CFLAGS -Wno-error)
-	# add matrix tests
-	add_subdirectory(matrix)
 endif()
 
 # use custom matrix lib instead of Eigen

src/modules/local_position_estimator/matrix/.gitignore

@@ -1 +0,0 @@
-build*/

src/modules/local_position_estimator/matrix/CMakeLists.txt

@@ -1,13 +0,0 @@
-cmake_minimum_required(VERSION 2.8)
-
-project(matrix CXX)
-
-set(CMAKE_BUILD_TYPE "RelWithDebInfo")
-
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Weffc++")
-
-enable_testing()
-
-include_directories(src)
-
-add_subdirectory(test)

src/modules/local_position_estimator/matrix/src/Matrix.hpp

@@ -1,464 +0,0 @@
-#pragma once
-
-#include <stdio.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include <math.h>
-
-namespace matrix
-{
-
-template<typename T, size_t  M, size_t N>
-class Matrix
-{
-
-private:
-	T _data[M][N];
-	size_t _rows;
-	size_t _cols;
-
-public:
-
-	Matrix() :
-		_data(),
-		_rows(M),
-		_cols(N)
-	{
-	}
-
-	Matrix(const T *data) :
-		_data(),
-		_rows(M),
-		_cols(N)
-	{
-		memcpy(_data, data, sizeof(_data));
-	}
-
-	Matrix(const Matrix &other) :
-		_data(),
-		_rows(M),
-		_cols(N)
-	{
-		memcpy(_data, other._data, sizeof(_data));
-	}
-
-	Matrix(T x, T y, T z) :
-		_data(),
-		_rows(M),
-		_cols(N)
-	{
-		// TODO, limit to only 3x1 matrices
-		_data[0][0] = x;
-		_data[1][0] = y;
-		_data[2][0] = z;
-	}
-
-	/**
-	 * Accessors/ Assignment etc.
-	 */
-
-	T *data()
-	{
-		return _data[0];
-	}
-
-	inline size_t rows() const
-	{
-		return _rows;
-	}
-
-	inline size_t cols() const
-	{
-		return _rows;
-	}
-
-	inline T operator()(size_t i) const
-	{
-		return _data[i][0];
-	}
-
-	inline T operator()(size_t i, size_t j) const
-	{
-		return _data[i][j];
-	}
-
-	inline T &operator()(size_t i)
-	{
-		return _data[i][0];
-	}
-
-	inline T &operator()(size_t i, size_t j)
-	{
-		return _data[i][j];
-	}
-
-	/**
-	 * Matrix Operations
-	 */
-
-	// this might use a lot of programming memory
-	// since it instantiates a class for every
-	// required mult pair, but it provides
-	// compile time size_t checking
-	template<size_t P>
-	Matrix<T, M, P> operator*(const Matrix<T, N, P> &other) const
-	{
-		const Matrix<T, M, N> &self = *this;
-		Matrix<T, M, P> res;
-		res.setZero();
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t k = 0; k < P; k++) {
-				for (size_t j = 0; j < N; j++) {
-					res(i, k) += self(i, j) * other(j, k);
-				}
-			}
-		}
-
-		return res;
-	}
-
-	Matrix<T, M, N> operator+(const Matrix<T, M, N> &other) const
-	{
-		Matrix<T, M, N> res;
-		const Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t j = 0; j < N; j++) {
-				res(i , j) = self(i, j) + other(i, j);
-			}
-		}
-
-		return res;
-	}
-
-	bool operator==(const Matrix<T, M, N> &other) const
-	{
-		Matrix<T, M, N> res;
-		const Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t j = 0; j < N; j++) {
-				if (self(i , j) != other(i, j)) {
-					return false;
-				}
-			}
-		}
-
-		return true;
-	}
-
-	Matrix<T, M, N> operator-(const Matrix<T, M, N> &other) const
-	{
-		Matrix<T, M, N> res;
-		const Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t j = 0; j < N; j++) {
-				res(i , j) = self(i, j) - other(i, j);
-			}
-		}
-
-		return res;
-	}
-
-	void operator+=(const Matrix<T, M, N> &other)
-	{
-		Matrix<T, M, N> &self = *this;
-		self = self + other;
-	}
-
-	void operator-=(const Matrix<T, M, N> &other)
-	{
-		Matrix<T, M, N> &self = *this;
-		self = self - other;
-	}
-
-	void operator*=(const Matrix<T, M, N> &other)
-	{
-		Matrix<T, M, N> &self = *this;
-		self = self * other;
-	}
-
-	/**
-	 * Scalar Operations
-	 */
-
-	Matrix<T, M, N> operator*(T scalar) const
-	{
-		Matrix<T, M, N> res;
-		const Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t j = 0; j < N; j++) {
-				res(i , j) = self(i, j) * scalar;
-			}
-		}
-
-		return res;
-	}
-
-	Matrix<T, M, N> operator+(T scalar) const
-	{
-		Matrix<T, M, N> res;
-		Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t j = 0; j < N; j++) {
-				res(i , j) = self(i, j) + scalar;
-			}
-		}
-
-		return res;
-	}
-
-	void operator*=(T scalar)
-	{
-		Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t j = 0; j < N; j++) {
-				self(i, j) = self(i, j) * scalar;
-			}
-		}
-	}
-
-	void operator/=(T scalar)
-	{
-		Matrix<T, M, N> &self = *this;
-		self = self * (1.0f / scalar);
-	}
-
-	/**
-	 * Misc. Functions
-	 */
-
-	void print()
-	{
-		Matrix<T, M, N> &self = *this;
-		printf("\n");
-
-		for (size_t i = 0; i < M; i++) {
-			printf("[");
-
-			for (size_t j = 0; j < N; j++) {
-				printf("%10g\t", double(self(i, j)));
-			}
-
-			printf("]\n");
-		}
-	}
-
-	Matrix<T, N, M> transpose() const
-	{
-		Matrix<T, N, M> res;
-		const Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t j = 0; j < N; j++) {
-				res(j, i) = self(i, j);
-			}
-		}
-
-		return res;
-	}
-
-	Matrix<T, M, M> expm(float dt, size_t n) const
-	{
-		Matrix<float, M, M> res;
-		res.setIdentity();
-		Matrix<float, M, M> A_pow = *this;
-		float k_fact = 1;
-		size_t k = 1;
-
-		while (k < n) {
-			res += A_pow * (float(pow(dt, k)) / k_fact);
-
-			if (k == n) { break; }
-
-			A_pow *= A_pow;
-			k_fact *= k;
-			k++;
-		}
-
-		return res;
-	}
-
-	Matrix<T, M, 1> diagonal() const
-	{
-		Matrix<T, M, 1> res;
-		// force square for now
-		const Matrix<T, M, M> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			res(i) = self(i, i);
-		}
-
-		return res;
-	}
-
-	void setZero()
-	{
-		memset(_data, 0, sizeof(_data));
-	}
-
-	void setIdentity()
-	{
-		setZero();
-		Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < M and i < N; i++) {
-			self(i, i) = 1;
-		}
-	}
-
-	inline void swapRows(size_t a, size_t b)
-	{
-		if (a == b) { return; }
-
-		Matrix<T, M, N> &self = *this;
-
-		for (size_t j = 0; j < cols(); j++) {
-			T tmp = self(a, j);
-			self(a, j) = self(b, j);
-			self(b, j) = tmp;
-		}
-	}
-
-	inline void swapCols(size_t a, size_t b)
-	{
-		if (a == b) { return; }
-
-		Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < rows(); i++) {
-			T tmp = self(i, a);
-			self(i, a) = self(i, b);
-			self(i, b) = tmp;
-		}
-	}
-
-	/**
-	 * inverse based on LU factorization with partial pivotting
-	 */
-	Matrix <T, M, M> inverse() const
-	{
-		Matrix<T, M, M> L;
-		L.setIdentity();
-		const Matrix<T, M, M> &A = (*this);
-		Matrix<T, M, M> U = A;
-		Matrix<T, M, M> P;
-		P.setIdentity();
-
-		//printf("A:\n"); A.print();
-
-		// for all diagonal elements
-		for (size_t n = 0; n < N; n++) {
-
-			// if diagonal is zero, swap with row below
-			if (fabsf(U(n, n)) < 1e-8f) {
-				//printf("trying pivot for row %d\n",n);
-				for (size_t i = 0; i < N; i++) {
-					if (i == n) { continue; }
-
-					//printf("\ttrying row %d\n",i);
-					if (fabsf(U(i, n)) > 1e-8f) {
-						//printf("swapped %d\n",i);
-						U.swapRows(i, n);
-						P.swapRows(i, n);
-					}
-				}
-			}
-
-#ifdef MATRIX_ASSERT
-			//printf("A:\n"); A.print();
-			//printf("U:\n"); U.print();
-			//printf("P:\n"); P.print();
-			//fflush(stdout);
-			ASSERT(fabsf(U(n, n)) > 1e-8f);
-#endif
-
-			// failsafe, return zero matrix
-			if (fabsf(U(n, n)) < 1e-8f) {
-				Matrix<T, M, M> zero;
-				zero.setZero();
-				return zero;
-			}
-
-			// for all rows below diagonal
-			for (size_t i = (n + 1); i < N; i++) {
-				L(i, n) = U(i, n) / U(n, n);
-
-				// add i-th row and n-th row
-				// multiplied by: -a(i,n)/a(n,n)
-				for (size_t k = n; k < N; k++) {
-					U(i, k) -= L(i, n) * U(n, k);
-				}
-			}
-		}
-
-		//printf("L:\n"); L.print();
-		//printf("U:\n"); U.print();
-
-		// solve LY=P*I for Y by forward subst
-		Matrix<T, M, M> Y = P;
-
-		// for all columns of Y
-		for (size_t c = 0; c < N; c++) {
-			// for all rows of L
-			for (size_t i = 0; i < N; i++) {
-				// for all columns of L
-				for (size_t j = 0; j < i; j++) {
-					// for all existing y
-					// subtract the component they
-					// contribute to the solution
-					Y(i, c) -= L(i, j) * Y(j, c);
-				}
-
-				// divide by the factor
-				// on current
-				// term to be solved
-				// Y(i,c) /= L(i,i);
-				// but L(i,i) = 1.0
-			}
-		}
-
-		//printf("Y:\n"); Y.print();
-
-		// solve Ux=y for x by back subst
-		Matrix<T, M, M> X = Y;
-
-		// for all columns of X
-		for (size_t c = 0; c < N; c++) {
-			// for all rows of U
-			for (size_t k = 0; k < N; k++) {
-				// have to go in reverse order
-				size_t i = N - 1 - k;
-
-				// for all columns of U
-				for (size_t j = i + 1; j < N; j++) {
-					// for all existing x
-					// subtract the component they
-					// contribute to the solution
-					X(i, c) -= U(i, j) * X(j, c);
-				}
-
-				// divide by the factor
-				// on current
-				// term to be solved
-				X(i, c) /= U(i, i);
-			}
-		}
-
-		//printf("X:\n"); X.print();
-		return X;
-	}
-
-};
-
-typedef Matrix<float, 2, 1> Vector2f;
-typedef Matrix<float, 3, 1> Vector3f;
-typedef Matrix<float, 3, 3> Matrix3f;
-
-}; // namespace matrix

src/modules/local_position_estimator/matrix/test/CMakeLists.txt

@@ -1,15 +0,0 @@
-set(tests
-	setIdentity
-	inverse
-	matrixMult
-	vectorAssignment
-	matrixAssignment
-	matrixScalarMult
-	transpose
-	)
-
-foreach(test ${tests})
-	add_executable(${test}
-		${test}.cpp)
-	add_test(${test} ${test})
-endforeach()

src/modules/local_position_estimator/matrix/test/inverse.cpp

@@ -1,30 +0,0 @@
-#include "Matrix.hpp"
-#include <assert.h>
-#include <stdio.h>
-
-using namespace matrix;
-
-int main()
-{
-	float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
-	Matrix3f A(data);
-	Matrix3f A_I = A.inverse();
-	float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
-	Matrix3f A_I_check(data_check);
-	(void)A_I;
-	assert(A_I == A_I_check);
-
-	// stess test
-	static const size_t n = 100;
-	Matrix<float, n, n> A_large;
-	A_large.setIdentity();
-	Matrix<float, n, n> A_large_I;
-	A_large_I.setZero();
-
-	for (size_t i = 0; i < 100; i++) {
-		A_large_I = A_large.inverse();
-		assert(A_large == A_large_I);
-	}
-
-	return 0;
-}

src/modules/local_position_estimator/matrix/test/matrixAssignment.cpp

@@ -1,29 +0,0 @@
-#include "Matrix.hpp"
-#include <assert.h>
-
-using namespace matrix;
-
-int main()
-{
-	Matrix3f m;
-	m.setZero();
-	m(0, 0) = 1;
-	m(0, 1) = 2;
-	m(0, 2) = 3;
-	m(1, 0) = 4;
-	m(1, 1) = 5;
-	m(1, 2) = 6;
-	m(2, 0) = 7;
-	m(2, 1) = 8;
-	m(2, 2) = 9;
-
-	m.print();
-
-	float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
-	Matrix3f m2(data);
-	m2.print();
-
-	assert(m == m2);
-
-	return 0;
-}

src/modules/local_position_estimator/matrix/test/matrixMult.cpp

@@ -1,26 +0,0 @@
-#include "Matrix.hpp"
-#include <assert.h>
-#include <stdio.h>
-
-using namespace matrix;
-
-int main()
-{
-	float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
-	Matrix3f A(data);
-	float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
-	Matrix3f A_I(data_check);
-	Matrix3f I;
-	I.setIdentity();
-	A.print();
-	A_I.print();
-	Matrix3f R = A * A_I;
-	R.print();
-	assert(R == I);
-
-	Matrix3f R2 = A;
-	R2 *= A_I;
-	R2.print();
-	assert(R2 == I);
-	return 0;
-}

src/modules/local_position_estimator/matrix/test/matrixScalarMult.cpp

@@ -1,18 +0,0 @@
-#include "Matrix.hpp"
-#include <assert.h>
-#include <stdio.h>
-
-using namespace matrix;
-
-int main()
-{
-	float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
-	Matrix3f A(data);
-	A = A * 2;
-	float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
-	Matrix3f A_check(data_check);
-	A.print();
-	A_check.print();
-	assert(A == A_check);
-	return 0;
-}

src/modules/local_position_estimator/matrix/test/setIdentity.cpp

@@ -1,25 +0,0 @@
-#include "Matrix.hpp"
-#include <assert.h>
-
-using namespace matrix;
-
-int main()
-{
-	Matrix3f A;
-	A.setIdentity();
-	assert(A.rows() == 3);
-	assert(A.cols() == 3);
-
-	for (int i = 0; i < 3; i++) {
-		for (int j = 0; j < 3; j++) {
-			if (i == j) {
-				assert(A(i, j) == 1);
-
-			} else {
-				assert(A(i, j) == 0);
-			}
-		}
-	}
-
-	return 0;
-}

src/modules/local_position_estimator/matrix/test/transpose.cpp

@@ -1,18 +0,0 @@
-#include "Matrix.hpp"
-#include <assert.h>
-#include <stdio.h>
-
-using namespace matrix;
-
-int main()
-{
-	float data[9] = {1, 2, 3, 4, 5, 6};
-	Matrix<float, 2, 3> A(data);
-	Matrix<float, 3, 2> A_T = A.transpose();
-	float data_check[9] = {1, 4, 2, 5, 3, 6};
-	Matrix<float, 3, 2> A_T_check(data_check);
-	A_T.print();
-	A_T_check.print();
-	assert(A_T == A_T_check);
-	return 0;
-}

src/modules/local_position_estimator/matrix/test/vectorAssignment.cpp

@@ -1,28 +0,0 @@
-#include "Matrix.hpp"
-#include <assert.h>
-
-using namespace matrix;
-
-int main()
-{
-	Vector3f v;
-	v(0) = 1;
-	v(1) = 2;
-	v(2) = 3;
-
-	v.print();
-
-	assert(v(0) == 1);
-	assert(v(1) == 2);
-	assert(v(2) == 3);
-
-	Vector3f v2(4, 5, 6);
-
-	v2.print();
-
-	assert(v2(0) == 4);
-	assert(v2(1) == 5);
-	assert(v2(2) == 6);
-
-	return 0;
-}

src/platforms/common/CMakeLists.txt

@@ -35,6 +35,7 @@ set(depends
 	prebuild_targets
 	git_mavlink
 	git_uavcan
+	git_matrix
 	)
 
 px4_add_module(