mirror of https://github.com/ArduPilot/ardupilot
158 lines
3.7 KiB
C++
158 lines
3.7 KiB
C++
//
|
|
// 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();
|
|
|
|
#define MAT_ALG_ACCURACY 1e-4f
|
|
|
|
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;
|
|
}
|
|
|
|
|
|
static void show_matrix(float *A, int n) {
|
|
for (int i = 0; i < n; i++) {
|
|
for (int j = 0; j < n; j++)
|
|
printf("%.10f ", A[i * n + j]);
|
|
printf("\n");
|
|
}
|
|
}
|
|
|
|
static bool compare_mat(const float *A, const float *B, const 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]) > MAT_ALG_ACCURACY) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
static void test_matrix_inverse(void)
|
|
{
|
|
//fast inverses
|
|
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];
|
|
|
|
|
|
//Test for 3x3 matrix
|
|
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");
|
|
return;
|
|
|
|
}
|
|
printf("\n\n3x3 Test Matrix:\n");
|
|
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
|
|
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("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
|
|
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("5x5 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("\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");
|
|
}
|
|
|
|
|
|
void setup(void)
|
|
{
|
|
hal.console->printf("Matrix Algebra test\n\n");
|
|
test_matrix_inverse();
|
|
hal.console->printf("Matrix Algebra tests done\n\n");
|
|
}
|
|
|
|
void loop(void) {}
|
|
|
|
AP_HAL_MAIN();
|