AP_Math: add inverse matrix test example

fix example build

libraries/AP_Math/examples/eulers/make.inc

@@ -8,6 +8,7 @@ LIBRARIES += AP_Compass
 LIBRARIES += AP_Declination
 LIBRARIES += AP_GPS
 LIBRARIES += AP_InertialSensor
+LIBRARIES += AP_AccelCal
 LIBRARIES += AP_Math
 LIBRARIES += AP_Mission
 LIBRARIES += AP_NavEKF

libraries/AP_Math/examples/location/make.inc

@@ -8,6 +8,7 @@ LIBRARIES += AP_Compass
 LIBRARIES += AP_Declination
 LIBRARIES += AP_GPS
 LIBRARIES += AP_InertialSensor
+LIBRARIES += AP_AccelCal
 LIBRARIES += AP_Math
 LIBRARIES += AP_Mission
 LIBRARIES += AP_NavEKF

libraries/AP_Math/examples/matrix_alg/Makefile

@@ -0,0 +1 @@
+include ../../../../mk/apm.mk

libraries/AP_Math/examples/matrix_alg/make.inc

@@ -0,0 +1,27 @@
+LIBRARIES += AP_Common
+LIBRARIES += AP_Progmem
+LIBRARIES += AP_Param
+LIBRARIES += AP_Math
+LIBRARIES += Filter
+LIBRARIES += AP_ADC
+LIBRARIES += AP_Compass
+LIBRARIES += AP_Baro
+LIBRARIES += AP_Notify
+LIBRARIES += AP_InertialSensor
+LIBRARIES += AP_AccelCal
+LIBRARIES += AP_GPS
+LIBRARIES += DataFlash
+LIBRARIES += GCS_MAVLink
+LIBRARIES += AP_Mission
+LIBRARIES += StorageManager
+LIBRARIES += AP_Terrain
+LIBRARIES += AP_Declination
+LIBRARIES += AP_AHRS
+LIBRARIES += AP_NavEKF
+LIBRARIES += AP_Airspeed
+LIBRARIES += AP_Vehicle
+LIBRARIES += AP_ADC_AnalogSource
+LIBRARIES += AP_Rally
+LIBRARIES += AP_BattMonitor
+LIBRARIES += AP_RangeFinder
+LIBRARIES += AP_OpticalFlow

libraries/AP_Math/examples/matrix_alg/matrix_alg.cpp

@@ -0,0 +1,124 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+//
+// Unit tests for the AP_Math rotations code
+//
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Math/AP_Math.h>
+#include <stdio.h>
+const AP_HAL::HAL& hal = AP_HAL::get_HAL();
+
+static uint16_t get_random(void)
+{
+    static uint32_t m_z = 1234;
+    static uint32_t m_w = 76542;
+    m_z = 36969 * (m_z & 65535) + (m_z >> 16);
+    m_w = 18000 * (m_w & 65535) + (m_w >> 16);
+    return ((m_z << 16) + m_w) & 0xF;
+}
+
+
+void show_matrix(float *A, int n) {
+    for (int i = 0; i < n; i++) {
+        for (int j = 0; j < n; j++)
+            printf("%2.5f  ", A[i * n + j]);
+        printf("\n");
+    }
+}
+
+bool compare_mat(float *A, float *B, uint8_t n)
+{
+    for(uint8_t i = 0; i < n; i++) {
+        for(uint8_t j = 0; j < n; j++) {
+            if(fabsf(A[i*n + j] - B[i*n + j]) > 1e-4f) {
+                return false;
+            }
+        }
+    }
+    return true;
+}
+
+static void test_matrix_inverse(void)
+{
+    //fast inverses
+    float test_mat[25];
+    for(uint8_t i = 0;i<25;i++) {
+        test_mat[i] = pow(-1,i)*get_random()/0.7f;
+    }
+    float mat[25];
+    uint8_t l = 0;
+
+    //Test for 3x3 matrix
+    l = 0;
+    if(inverse(test_mat,mat,3)){
+        inverse(mat,mat,3);
+    } else {
+        hal.console->printf("3x3 Matrix is Singular!\n");
+        return;
+
+    }
+    printf("\n\n3x3 Test Matrix:\n");
+    show_matrix(test_mat,3);
+    printf("\nInverse of Inverse of matrix\n");
+    show_matrix(mat,3);
+    printf("\n");
+    //compare matrix
+    if(!compare_mat(test_mat,mat,3)) {
+        printf("Test Failed!!\n");
+        return;
+    }
+
+    //Test for 4x4 matrix
+    l = 0;
+    if(inverse(test_mat,mat,4)){
+        inverse(mat,mat,4);
+    } else {
+        hal.console->printf("3x3 Matrix is Singular!\n");
+        return;
+
+    }
+    printf("\n\n4x4 Test Matrix:\n");
+    show_matrix(test_mat,4);
+    printf("\nInverse of Inverse of matrix\n");
+    show_matrix(mat,4);
+    printf("\n");
+    if(!compare_mat(test_mat,mat,4)) {
+        printf("Test Failed!!\n");
+        return;
+    }
+
+    //Test for 5x5 matrix
+    l = 0;
+    if(inverse(test_mat,mat,5)) {
+        inverse(mat,mat,5);
+    } else {
+        hal.console->printf("3x3 Matrix is Singular!\n");
+        return;
+
+    }
+
+    printf("\n\n5x5 Test Matrix:\n");
+    show_matrix(test_mat,5);
+    printf("\nInverse of Inverse of matrix\n");
+    show_matrix(mat,5);
+    printf("\n");
+    if(!compare_mat(test_mat,mat,5)) {
+        printf("Test Failed!!\n");
+        return;
+    }
+
+
+    hal.console->printf("All tests succeeded!!\n");
+}
+
+
+void setup(void)
+{
+    hal.console->println("Matrix Algebra test\n");
+    test_matrix_inverse();
+    hal.console->println("Matrix Algebra tests done\n");
+}
+
+void loop(void) {}
+
+AP_HAL_MAIN();

libraries/AP_Math/examples/polygon/make.inc

@@ -8,6 +8,7 @@ LIBRARIES += AP_Compass
 LIBRARIES += AP_Declination
 LIBRARIES += AP_GPS
 LIBRARIES += AP_InertialSensor
+LIBRARIES += AP_AccelCal
 LIBRARIES += AP_Math
 LIBRARIES += AP_Mission
 LIBRARIES += AP_NavEKF

libraries/AP_Math/examples/rotations/make.inc

@@ -7,6 +7,7 @@ LIBRARIES += AP_Compass
 LIBRARIES += AP_Baro
 LIBRARIES += AP_Notify
 LIBRARIES += AP_InertialSensor
+LIBRARIES += AP_AccelCal
 LIBRARIES += AP_GPS
 LIBRARIES += DataFlash
 LIBRARIES += GCS_MAVLink