From a0c3cbffee764147857bd082e7c4dabe3e6fdaa5 Mon Sep 17 00:00:00 2001 From: Siddharth Bharat Purohit Date: Mon, 26 Oct 2015 17:51:43 -0700 Subject: [PATCH] AP_Math: add inverse matrix test example fix example build --- libraries/AP_Math/examples/eulers/make.inc | 1 + libraries/AP_Math/examples/location/make.inc | 1 + .../AP_Math/examples/matrix_alg/Makefile | 1 + .../AP_Math/examples/matrix_alg/make.inc | 27 ++++ .../examples/matrix_alg/matrix_alg.cpp | 124 ++++++++++++++++++ .../examples/matrix_alg/nocore.inoflag | 0 libraries/AP_Math/examples/polygon/make.inc | 1 + libraries/AP_Math/examples/rotations/make.inc | 1 + 8 files changed, 156 insertions(+) create mode 100644 libraries/AP_Math/examples/matrix_alg/Makefile create mode 100644 libraries/AP_Math/examples/matrix_alg/make.inc create mode 100644 libraries/AP_Math/examples/matrix_alg/matrix_alg.cpp create mode 100644 libraries/AP_Math/examples/matrix_alg/nocore.inoflag diff --git a/libraries/AP_Math/examples/eulers/make.inc b/libraries/AP_Math/examples/eulers/make.inc index ff6d836dde..35ce8eefbd 100644 --- a/libraries/AP_Math/examples/eulers/make.inc +++ b/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 diff --git a/libraries/AP_Math/examples/location/make.inc b/libraries/AP_Math/examples/location/make.inc index bdd71095c3..bc91287fcf 100644 --- a/libraries/AP_Math/examples/location/make.inc +++ b/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 diff --git a/libraries/AP_Math/examples/matrix_alg/Makefile b/libraries/AP_Math/examples/matrix_alg/Makefile new file mode 100644 index 0000000000..f5daf25151 --- /dev/null +++ b/libraries/AP_Math/examples/matrix_alg/Makefile @@ -0,0 +1 @@ +include ../../../../mk/apm.mk diff --git a/libraries/AP_Math/examples/matrix_alg/make.inc b/libraries/AP_Math/examples/matrix_alg/make.inc new file mode 100644 index 0000000000..950edfcc7a --- /dev/null +++ b/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 diff --git a/libraries/AP_Math/examples/matrix_alg/matrix_alg.cpp b/libraries/AP_Math/examples/matrix_alg/matrix_alg.cpp new file mode 100644 index 0000000000..fee860739c --- /dev/null +++ b/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 +#include +#include +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(); diff --git a/libraries/AP_Math/examples/matrix_alg/nocore.inoflag b/libraries/AP_Math/examples/matrix_alg/nocore.inoflag new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_Math/examples/polygon/make.inc b/libraries/AP_Math/examples/polygon/make.inc index bdd71095c3..bc91287fcf 100644 --- a/libraries/AP_Math/examples/polygon/make.inc +++ b/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 diff --git a/libraries/AP_Math/examples/rotations/make.inc b/libraries/AP_Math/examples/rotations/make.inc index 10147f7261..2568a55862 100644 --- a/libraries/AP_Math/examples/rotations/make.inc +++ b/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