Update matrix submodule and add pseudoinverse tests (tests matrix)

boards/holybro/durandal-v1/stackcheck.cmake

@@ -107,13 +107,13 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
-		bottle_drop # OBC challenge
+		# bottle_drop # OBC challenge
-		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
+		# fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
-		hello
+		# hello
-		hwtest # Hardware test
+		# hwtest # Hardware test
 		#matlab_csv_serial
-		px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html
+		# px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html
-		px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html
+		# px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html
-		rover_steering_control # Rover example app
+		# rover_steering_control # Rover example app
-		uuv_example_app
+		# uuv_example_app
 	)

boards/px4/fmu-v2/fixedwing.cmake

@@ -23,9 +23,9 @@ px4_add_board(
 		adc
 		#barometer # all available barometer drivers
 		barometer/ms5611
-		batt_smbus
+		# batt_smbus
-		camera_capture
+		# camera_capture
-		camera_trigger
+		# camera_trigger
 		differential_pressure # all available differential pressure drivers
 		distance_sensor # all available distance sensor drivers
 		gps
@@ -48,7 +48,7 @@ px4_add_board(
 		commander
 		dataman
 		ekf2
-		events
+		# events
 		fw_att_control
 		fw_pos_control_l1
 		land_detector
@@ -67,7 +67,7 @@ px4_add_board(
 		#dumpfile
 		#esc_calib
 		hardfault_log
-		i2cdetect
+		# i2cdetect
 		#led_control
 		mixer
 		#motor_ramp
@@ -82,8 +82,8 @@ px4_add_board(
 		top
 		#topic_listener
 		tune_control
-		usb_connected
+		# usb_connected
 		ver
-		work_queue
+		# work_queue
 
 	)

boards/px4/fmu-v2/multicopter.cmake

@@ -22,8 +22,8 @@ px4_add_board(
 		adc
 		barometer/ms5611
 		#batt_smbus
-		camera_capture
+		# camera_capture
-		camera_trigger
+		# camera_trigger
 		distance_sensor # all available distance sensor drivers
 		gps
 		imu/l3gd20
@@ -80,7 +80,7 @@ px4_add_board(
 		top
 		#topic_listener
 		tune_control
-		usb_connected
+		# usb_connected
 		ver
 		#work_queue
 

boards/px4/fmu-v2/test.cmake

@@ -60,7 +60,7 @@ px4_add_board(
 
 	MODULES
 		#attitude_estimator_q
-		camera_feedback
+		# camera_feedback
 		commander
 		dataman
 		#ekf2
@@ -68,8 +68,8 @@ px4_add_board(
 		#fw_att_control
 		#fw_pos_control_l1
 		#rover_pos_control
-		land_detector
+		# land_detector
-		landing_target_estimator
+		# landing_target_estimator
 		load_mon
 		#local_position_estimator
 		logger
@@ -81,7 +81,7 @@ px4_add_board(
 		battery_status
 		sensors
 		vmount
-		vtol_att_control
+		# vtol_att_control
 		#airspeed_selector
 
 	SYSTEMCMDS

boards/px4/fmu-v5/stackcheck.cmake

@@ -106,7 +106,7 @@ px4_add_board(
 		reflect
 		sd_bench
 		shutdown
-		tests # tests and test runner
+		# tests # tests and test runner
 		top
 		topic_listener
 		tune_control

src/systemcmds/tests/test_matrix.cpp

@@ -68,6 +68,7 @@ private:
 	bool vector3Tests();
 	bool vectorAssignmentTests();
 	bool dcmRenormTests();
+	bool pseudoInverseTests();
 };
 
 bool MatrixTest::run_tests()
@@ -89,6 +90,7 @@ bool MatrixTest::run_tests()
 	ut_run_test(vector3Tests);
 	ut_run_test(vectorAssignmentTests);
 	ut_run_test(dcmRenormTests);
+	ut_run_test(pseudoInverseTests);
 
 	return (_tests_failed == 0);
 }
@@ -757,3 +759,98 @@ bool MatrixTest::dcmRenormTests()
 
 	return true;
 }
+
+bool MatrixTest::pseudoInverseTests()
+{
+	// 3x4 Matrix test
+	float data0[12] = {
+		0.f, 1.f,  2.f,  3.f,
+		4.f, 5.f,  6.f,  7.f,
+		8.f, 9.f, 10.f, 11.f
+	};
+
+	float data0_check[12] = {
+		-0.3375f, -0.1f,  0.1375f,
+		-0.13333333f, -0.03333333f,  0.06666667f,
+		0.07083333f,  0.03333333f, -0.00416667f,
+		0.275f, 0.1f, -0.075f
+	};
+
+	Matrix<float, 3, 4> A0(data0);
+	Matrix<float, 4, 3> A0_I = geninv(A0);
+	Matrix<float, 4, 3> A0_I_check(data0_check);
+
+	ut_test((A0_I - A0_I_check).abs().max() < 1e-5);
+
+	// 4x3 Matrix test
+	float data1[12] = {
+		0.f, 4.f, 8.f,
+		1.f, 5.f, 9.f,
+		2.f, 6.f, 10.f,
+		3.f, 7.f, 11.f
+	};
+
+	float data1_check[12] = {
+		-0.3375f, -0.13333333f,  0.07083333f,  0.275f,
+		-0.1f, -0.03333333f,  0.03333333f,  0.1f,
+		0.1375f,  0.06666667f, -0.00416667f, -0.075f
+	};
+
+	Matrix<float, 4, 3> A1(data1);
+	Matrix<float, 3, 4> A1_I = geninv(A1);
+	Matrix<float, 3, 4> A1_I_check(data1_check);
+
+	ut_test((A1_I - A1_I_check).abs().max() < 1e-5);
+
+	// Square matrix test
+	float data2[9] = {
+		0, 2, 3,
+		4, 5, 6,
+		7, 8, 10
+	};
+	float data2_check[9] = {
+		-0.4f, -0.8f,  0.6f,
+		-0.4f,  4.2f, -2.4f,
+		0.6f, -2.8f,  1.6f
+	};
+
+	SquareMatrix<float, 3> A2(data2);
+	SquareMatrix<float, 3> A2_I = inv(A2);
+	SquareMatrix<float, 3> A2_I_check(data2_check);
+	ut_test((A2_I - A2_I_check).abs().max() < 1e-5);
+
+	// Mock-up effectiveness matrix
+	const float B_quad_w[6][16] = {
+		{-0.5717536f,  0.43756646f,  0.5717536f, -0.43756646f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 0.35355328f, -0.35355328f,  0.35355328f, -0.35355328f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 0.28323701f,  0.28323701f, -0.28323701f, -0.28323701f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 0.f,  0.f,  0.f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 0.f,  0.f,  0.f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
+	};
+	Matrix<float, 6, 16> B(B_quad_w);
+	const float A_quad_w[16][6] = {
+		{ -0.495383f,  0.707107f,  0.765306f,  0.0f, 0.0f, -1.000000f },
+		{  0.495383f, -0.707107f,  1.000000f,  0.0f, 0.0f, -1.000000f },
+		{  0.495383f,  0.707107f, -0.765306f,  0.0f, 0.0f, -1.000000f },
+		{ -0.495383f, -0.707107f, -1.000000f,  0.0f, 0.0f, -1.000000f },
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}
+	};
+	Matrix<float, 16, 6> A_check(A_quad_w);
+	Matrix<float, 16, 6> A = geninv(B);
+	ut_test((A - A_check).abs().max() < 1e-5);
+
+	return true;
+
+}

src/systemcmds/tests/test_microbench_matrix.cpp

@@ -101,7 +101,9 @@ private:
 	matrix::Quatf q;
 	matrix::Eulerf e;
 	matrix::Dcmf d;
-
+	matrix::Matrix<float, 16, 6> A16;
+	matrix::Matrix<float, 6, 16> B16;
+	matrix::Matrix<float, 6, 16> B16_4;
 };
 
 bool MicroBenchMatrix::run_tests()
@@ -126,6 +128,17 @@ void MicroBenchMatrix::reset()
 	q = matrix::Quatf(rand(), rand(), rand(), rand());
 	e = matrix::Eulerf(random(-2.0 * M_PI, 2.0 * M_PI), random(-2.0 * M_PI, 2.0 * M_PI), random(-2.0 * M_PI, 2.0 * M_PI));
 	d = q;
+
+	for (size_t j = 0; j < 6; j++) {
+		for (size_t i = 0; i < 16; i++) {
+			B16(j, i) = random(-10.0, 10.0);
+		}
+
+		for (size_t i = 0; i < 4; i++) {
+			B16_4(j, i) = random(-10.0, 10.0);
+		}
+	}
+
 }
 
 ut_declare_test_c(test_microbench_matrix, MicroBenchMatrix)
@@ -141,6 +154,9 @@ bool MicroBenchMatrix::time_px4_matrix()
 	PERF("matrix Dcm from Euler", d = e, 1000);
 	PERF("matrix Dcm from Quaternion", d = q, 1000);
 
+	PERF("matrix 6x16 pseudo inverse (all non-zero columns)", A16 = matrix::geninv(B16), 1000);
+	PERF("matrix 6x16 pseudo inverse (4 non-zero columns)", A16 = matrix::geninv(B16_4), 1000);
+
 	return true;
 }