Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: add inverse matrix test to check if inverse(mat)*mat = I

Browse Source 
where I is an identity matrix (a matrix with diagonal elements = 1)
This commit is contained in:
bugobliterator 2015-12-28 15:19:05 -08:00 committed by Jonathan Challinger
parent 65915bbe71
commit 1a4b4fa85e
3 changed files with 50 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_Math/AP_Math.h
View File

@ -115,6 +115,9 @@ bool inverse3x3(float m[], float invOut[]);
// invOut is an inverted 3x3 matrix when returns true, otherwise matrix is Singular
bool inverse4x4(float m[],float invOut[]);
// matrix multiplication of two NxN matrices
float* mat_mul(float *A, float *B, uint8_t n);
// see if location is past a line perpendicular to
// the line between point1 and point2. If point1 is
// our previous waypoint and point2 is our target waypoint

56
libraries/AP_Math/examples/matrix_alg/matrix_alg.cpp
View File

@ -8,6 +8,8 @@
#include <stdio.h>
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
#define MAT_ALG_ACCURACY 1e-4f
static uint16_t get_random(void)
{
static uint32_t m_z = 1234;
@ -21,7 +23,7 @@ static uint16_t get_random(void)
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("%.10f ", A[i * n + j]);
printf("\n");
}
}
@ -30,7 +32,7 @@ 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) {
if(fabsf(A[i*n + j] - B[i*n + j]) > MAT_ALG_ACCURACY) {
return false;
}
}
@ -41,16 +43,21 @@ bool compare_mat(float *A, float *B, uint8_t n)
static void test_matrix_inverse(void)
{
//fast inverses
float test_mat[25];
float test_mat[25],ident_mat[25];
float *out_mat;
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;
memset(ident_mat,0,sizeof(ident_mat));
for(uint8_t i=0; i<3; i++) {
ident_mat[i*3+i] = 1.0f;
}
if(inverse(test_mat,mat,3)){
out_mat = mat_mul(test_mat,mat,3);
inverse(mat,mat,3);
} else {
hal.console->printf("3x3 Matrix is Singular!\n");
@ -61,38 +68,60 @@ static void test_matrix_inverse(void)
show_matrix(test_mat,3);
printf("\nInverse of Inverse of matrix\n");
show_matrix(mat,3);
printf("\nInv(A) * A\n");
show_matrix(out_mat,3);
printf("\n");
//compare matrix
if(!compare_mat(test_mat,mat,3)) {
printf("Test Failed!!\n");
return;
}
if(!compare_mat(ident_mat,out_mat,3)) {
printf("Identity output Test Failed!!\n");
return;
}
//Test for 4x4 matrix
l = 0;
memset(ident_mat,0,sizeof(ident_mat));
for(uint8_t i=0; i<4; i++) {
ident_mat[i*4+i] = 1.0f;
}
if(inverse(test_mat,mat,4)){
out_mat = mat_mul(test_mat,mat,4);
inverse(mat,mat,4);
} else {
hal.console->printf("3x3 Matrix is Singular!\n");
hal.console->printf("4x4 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("\nInv(A) * A\n");
show_matrix(out_mat,4);
printf("\n");
if(!compare_mat(test_mat,mat,4)) {
printf("Test Failed!!\n");
return;
}
if(!compare_mat(ident_mat,out_mat,4)) {
printf("Identity output Test Failed!!\n");
return;
}
//Test for 5x5 matrix
l = 0;
memset(ident_mat,0,sizeof(ident_mat));
for(uint8_t i=0; i<5; i++) {
ident_mat[i*5+i] = 1.0f;
}
if(inverse(test_mat,mat,5)) {
out_mat = mat_mul(test_mat,mat,5);
inverse(mat,mat,5);
} else {
hal.console->printf("3x3 Matrix is Singular!\n");
hal.console->printf("5x5 Matrix is Singular!\n");
return;
}
@ -101,12 +130,17 @@ static void test_matrix_inverse(void)
show_matrix(test_mat,5);
printf("\nInverse of Inverse of matrix\n");
show_matrix(mat,5);
printf("\nInv(A) * A\n");
show_matrix(out_mat,5);
printf("\n");
if(!compare_mat(test_mat,mat,5)) {
printf("Test Failed!!\n");
return;
}
if(!compare_mat(ident_mat,out_mat,5)) {
printf("Identity output Test Failed!!\n");
return;
}
hal.console->printf("All tests succeeded!!\n");
}

2
libraries/AP_Math/matrix_alg.cpp
View File

@ -23,6 +23,8 @@
#include <stdio.h>
extern const AP_HAL::HAL& hal;
//TODO: use higher precision datatypes to achieve more accuracy for matrix algebra operations
/*
* Does matrix multiplication of two regular/square matrices
*