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

AP_Math: fixup matrix algorithms to be in cpp file

Browse Source 
this fixes an issue where optimize O2 was forced on any file that
included AP_Math.h. It also fixes the test suite for matrix_alg, and
fixes the type handling to be consistent
This commit is contained in:
Andrew Tridgell 2021-01-18 11:39:26 +11:00 committed by Peter Barker
parent 3b3e2c01f8
commit 999268cbba
4 changed files with 105 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_Math/AP_Math.h
View File

@ -82,23 +82,18 @@ float safe_asin(const T v);
template <typename T> template <typename T>
float safe_sqrt(const T v); float safe_sqrt(const T v);
// invOut is an inverted 4x4 matrix when returns true, otherwise matrix is Singular
template<typename T>
bool inverse3x3(const T m[], T invOut[]) WARN_IF_UNUSED;
// invOut is an inverted 3x3 matrix when returns true, otherwise matrix is Singular
template <typename T>
bool inverse4x4(const T m[],T invOut[]) WARN_IF_UNUSED;
// matrix multiplication of two NxN matrices // matrix multiplication of two NxN matrices
template <typename T> template <typename T>
float *mat_mul(const T *A, const T *B, uint8_t n); void mat_mul(const T *A, const T *B, T *C, uint16_t n);
// matrix algebra // matrix inverse
template <typename T> template <typename T>
bool inverse(const T x[], T y[], uint16_t dim) WARN_IF_UNUSED; bool mat_inverse(const T *x, T *y, uint16_t dim) WARN_IF_UNUSED;
// matrix identity
template <typename T>
void mat_identity(T *x, uint16_t dim);
#include "matrix_alg.h"
/* /*
* Constrain an angle to be within the range: -180 to 180 degrees. The second * Constrain an angle to be within the range: -180 to 180 degrees. The second
* parameter changes the units. Default: 1 == degrees, 10 == dezi, * parameter changes the units. Default: 1 == degrees, 10 == dezi,

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

@ -5,10 +5,16 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <stdio.h> #include <stdio.h>
void setup();
void loop();
const AP_HAL::HAL& hal = AP_HAL::get_HAL(); const AP_HAL::HAL& hal = AP_HAL::get_HAL();
#define MAT_ALG_ACCURACY 1e-4f #define MAT_ALG_ACCURACY 1e-4f
typedef float ftype;
static uint16_t get_random(void) static uint16_t get_random(void)
{ {
static uint32_t m_z = 1234; static uint32_t m_z = 1234;
@ -19,7 +25,7 @@ static uint16_t get_random(void)
} }
static void show_matrix(float *A, int n) { static void show_matrix(ftype *A, int n) {
for (int i = 0; i < n; i++) { for (int i = 0; i < n; i++) {
for (int j = 0; j < n; j++) for (int j = 0; j < n; j++)
printf("%.10f ", A[i * n + j]); printf("%.10f ", A[i * n + j]);
@ -27,7 +33,7 @@ static void show_matrix(float *A, int n) {
} }
} }
static bool compare_mat(const float *A, const float *B, const uint8_t n) static bool compare_mat(const ftype *A, const ftype *B, const uint8_t n)
{ {
for(uint8_t i = 0; i < n; i++) { for(uint8_t i = 0; i < n; i++) {
for(uint8_t j = 0; j < n; j++) { for(uint8_t j = 0; j < n; j++) {
@ -42,22 +48,17 @@ static bool compare_mat(const float *A, const float *B, const uint8_t n)
static void test_matrix_inverse(void) static void test_matrix_inverse(void)
{ {
//fast inverses //fast inverses
float test_mat[25],ident_mat[25]; ftype test_mat[25],ident_mat[25];
float *out_mat; ftype out_mat[25], out_mat2[25], mat[25];
for(uint8_t i = 0;i<25;i++) { for(uint8_t i = 0;i<25;i++) {
test_mat[i] = pow(-1,i)*get_random()/0.7f; test_mat[i] = powf(-1,i)*get_random()/0.7f;
} }
float mat[25];
//Test for 3x3 matrix //Test for 3x3 matrix
memset(ident_mat,0,sizeof(ident_mat)); mat_identity(ident_mat, 3);
for(uint8_t i=0; i<3; i++) { if (mat_inverse(test_mat,mat,3) && mat_inverse(mat, out_mat2, 3)) {
ident_mat[i*3+i] = 1.0f; mat_mul(test_mat, mat, out_mat, 3);
}
if(inverse(test_mat,mat,3)){
out_mat = mat_mul(test_mat,mat,3);
inverse(mat,mat,3);
} else { } else {
hal.console->printf("3x3 Matrix is Singular!\n"); hal.console->printf("3x3 Matrix is Singular!\n");
return; return;
@ -70,25 +71,22 @@ static void test_matrix_inverse(void)
printf("\nInv(A) * A\n"); printf("\nInv(A) * A\n");
show_matrix(out_mat,3); show_matrix(out_mat,3);
printf("\n"); printf("\n");
//compare matrix
if(!compare_mat(test_mat,mat,3)) { // compare matrix
if (!compare_mat(test_mat, out_mat2, 3)) {
printf("Test Failed!!\n"); printf("Test Failed!!\n");
return; return;
} }
if(!compare_mat(ident_mat,out_mat,3)) { if (!compare_mat(ident_mat, out_mat, 3)) {
printf("Identity output Test Failed!!\n"); printf("Identity output Test Failed!!\n");
return; return;
} }
//Test for 4x4 matrix //Test for 4x4 matrix
memset(ident_mat,0,sizeof(ident_mat)); mat_identity(ident_mat, 4);
for(uint8_t i=0; i<4; i++) { if (mat_inverse(test_mat, mat, 4) && mat_inverse(mat, out_mat2, 4)){
ident_mat[i*4+i] = 1.0f; mat_mul(test_mat, mat, out_mat, 4);
}
if(inverse(test_mat,mat,4)){
out_mat = mat_mul(test_mat,mat,4);
inverse(mat,mat,4);
} else { } else {
hal.console->printf("4x4 Matrix is Singular!\n"); hal.console->printf("4x4 Matrix is Singular!\n");
return; return;
@ -100,29 +98,22 @@ static void test_matrix_inverse(void)
printf("\nInv(A) * A\n"); printf("\nInv(A) * A\n");
show_matrix(out_mat,4); show_matrix(out_mat,4);
printf("\n"); printf("\n");
if(!compare_mat(test_mat,mat,4)) { if (!compare_mat(test_mat, out_mat2, 4)) {
printf("Test Failed!!\n"); printf("Test Failed!!\n");
return; return;
} }
if(!compare_mat(ident_mat,out_mat,4)) { if (!compare_mat(ident_mat,out_mat,4)) {
printf("Identity output Test Failed!!\n"); printf("Identity output Test Failed!!\n");
return; return;
} }
//Test for 5x5 matrix //Test for 5x5 matrix
memset(ident_mat,0,sizeof(ident_mat)); mat_identity(ident_mat, 5);
for(uint8_t i=0; i<5; i++) { if (mat_inverse(test_mat,mat,5) && mat_inverse(mat, out_mat2, 5)) {
ident_mat[i*5+i] = 1.0f; mat_mul(test_mat, mat, out_mat, 5);
}
if(inverse(test_mat,mat,5)) {
out_mat = mat_mul(test_mat,mat,5);
inverse(mat,mat,5);
} else { } else {
hal.console->printf("5x5 Matrix is Singular!\n"); hal.console->printf("5x5 Matrix is Singular!\n");
return; return;
} }
printf("\n\n5x5 Test Matrix:\n"); printf("\n\n5x5 Test Matrix:\n");
@ -132,11 +123,11 @@ static void test_matrix_inverse(void)
printf("\nInv(A) * A\n"); printf("\nInv(A) * A\n");
show_matrix(out_mat,5); show_matrix(out_mat,5);
printf("\n"); printf("\n");
if(!compare_mat(test_mat,mat,5)) { if (!compare_mat(test_mat, out_mat2, 5)) {
printf("Test Failed!!\n"); printf("Test Failed!!\n");
return; return;
} }
if(!compare_mat(ident_mat,out_mat,5)) { if (!compare_mat(ident_mat, out_mat, 5)) {
printf("Identity output Test Failed!!\n"); printf("Identity output Test Failed!!\n");
return; return;
} }

7
libraries/AP_Math/examples/matrix_alg/wscript Normal file
View File

@ -0,0 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
def build(bld):
bld.ap_example(
use='ap',
)

101
libraries/AP_Math/matrix_alg.h → libraries/AP_Math/matrix_alg.cpp
View File

@ -15,20 +15,15 @@
* You should have received a copy of the GNU General Public License along * You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>. * with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
#pragma GCC optimize("O2")
#pragma once
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include "AP_Math.h"
#include <stdio.h> #include <stdio.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <fenv.h> #include <fenv.h>
#endif #endif
// 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 * Does matrix multiplication of two regular/square matrices
* *
@ -38,14 +33,14 @@
* @returns multiplied matrix i.e. A*B * @returns multiplied matrix i.e. A*B
*/ */
template<typename T> template<typename T>
T* matrix_multiply(const T *A, const T *B, uint8_t n) static T* matrix_multiply(const T *A, const T *B, uint16_t n)
{ {
T* ret = new T[n*n]; T* ret = new T[n*n];
memset(ret,0.0f,n*n*sizeof(T)); memset(ret,0.0f,n*n*sizeof(T));
for(uint8_t i = 0; i < n; i++) { for(uint16_t i = 0; i < n; i++) {
for(uint8_t j = 0; j < n; j++) { for(uint16_t j = 0; j < n; j++) {
for(uint8_t k = 0;k < n; k++) { for(uint16_t k = 0;k < n; k++) {
ret[i*n + j] += A[i*n + k] * B[k*n + j]; ret[i*n + j] += A[i*n + k] * B[k*n + j];
} }
} }
@ -71,19 +66,19 @@ static inline void swap(T &a, T &b)
* @returns false = matrix is Singular or non positive definite, true = matrix inversion successful * @returns false = matrix is Singular or non positive definite, true = matrix inversion successful
*/ */
template<typename T> template<typename T>
static void mat_pivot(const T* A, T* pivot, uint8_t n) static void mat_pivot(const T* A, T* pivot, uint16_t n)
{ {
for(uint8_t i = 0;i<n;i++){ for(uint16_t i = 0;i<n;i++){
for(uint8_t j=0;j<n;j++) { for(uint16_t j=0;j<n;j++) {
pivot[i*n+j] = static_cast<T>(i==j); pivot[i*n+j] = static_cast<T>(i==j);
} }
} }
for(uint8_t i = 0;i < n; i++) { for(uint16_t i = 0;i < n; i++) {
uint8_t max_j = i; uint16_t max_j = i;
for(uint8_t j=i;j<n;j++){ for(uint16_t j=i;j<n;j++){
if (std::is_same<T, double>::value) { if (std::is_same<T, double>::value) {
if(fabs(A[j*n + i]) > fabs(A[max_j*n + i])) { if(fabsf(A[j*n + i]) > fabsf(A[max_j*n + i])) {
max_j = j; max_j = j;
} }
} else { } else {
@ -94,7 +89,7 @@ static void mat_pivot(const T* A, T* pivot, uint8_t n)
} }
if(max_j != i) { if(max_j != i) {
for(uint8_t k = 0; k < n; k++) { for(uint16_t k = 0; k < n; k++) {
swap(pivot[i*n + k], pivot[max_j*n + k]); swap(pivot[i*n + k], pivot[max_j*n + k]);
} }
} }
@ -109,7 +104,7 @@ static void mat_pivot(const T* A, T* pivot, uint8_t n)
* @param n, dimension of matrix * @param n, dimension of matrix
*/ */
template<typename T> template<typename T>
static void mat_forward_sub(const T *L, T *out, uint8_t n) static void mat_forward_sub(const T *L, T *out, uint16_t n)
{ {
// Forward substitution solve LY = I // Forward substitution solve LY = I
for(int i = 0; i < n; i++) { for(int i = 0; i < n; i++) {
@ -131,7 +126,7 @@ static void mat_forward_sub(const T *L, T *out, uint8_t n)
* @param n, dimension of matrix * @param n, dimension of matrix
*/ */
template<typename T> template<typename T>
static void mat_back_sub(const T *U, T *out, uint8_t n) static void mat_back_sub(const T *U, T *out, uint16_t n)
{ {
// Backward Substitution solve UY = I // Backward Substitution solve UY = I
for(int i = n-1; i >= 0; i--) { for(int i = n-1; i >= 0; i--) {
@ -154,7 +149,7 @@ static void mat_back_sub(const T *U, T *out, uint8_t n)
* @param n, dimension of matrix * @param n, dimension of matrix
*/ */
template<typename T> template<typename T>
static void mat_LU_decompose(const T* A, T* L, T* U, T *P, uint8_t n) static void mat_LU_decompose(const T* A, T* L, T* U, T *P, uint16_t n)
{ {
memset(L,0,n*n*sizeof(T)); memset(L,0,n*n*sizeof(T));
memset(U,0,n*n*sizeof(T)); memset(U,0,n*n*sizeof(T));
@ -162,20 +157,20 @@ static void mat_LU_decompose(const T* A, T* L, T* U, T *P, uint8_t n)
mat_pivot(A,P,n); mat_pivot(A,P,n);
T *APrime = matrix_multiply(P,A,n); T *APrime = matrix_multiply(P,A,n);
for(uint8_t i = 0; i < n; i++) { for(uint16_t i = 0; i < n; i++) {
L[i*n + i] = 1; L[i*n + i] = 1;
} }
for(uint8_t i = 0; i < n; i++) { for(uint16_t i = 0; i < n; i++) {
for(uint8_t j = 0; j < n; j++) { for(uint16_t j = 0; j < n; j++) {
if(j <= i) { if(j <= i) {
U[j*n + i] = APrime[j*n + i]; U[j*n + i] = APrime[j*n + i];
for(uint8_t k = 0; k < j; k++) { for(uint16_t k = 0; k < j; k++) {
U[j*n + i] -= L[j*n + k] * U[k*n + i]; U[j*n + i] -= L[j*n + k] * U[k*n + i];
} }
} }
if(j >= i) { if(j >= i) {
L[j*n + i] = APrime[j*n + i]; L[j*n + i] = APrime[j*n + i];
for(uint8_t k = 0; k < i; k++) { for(uint16_t k = 0; k < i; k++) {
L[j*n + i] -= L[j*n + k] * U[k*n + i]; L[j*n + i] -= L[j*n + k] * U[k*n + i];
} }
L[j*n + i] /= U[i*n + i]; L[j*n + i] /= U[i*n + i];
@ -195,7 +190,7 @@ static void mat_LU_decompose(const T* A, T* L, T* U, T *P, uint8_t n)
* @returns false = matrix is Singular, true = matrix inversion successful * @returns false = matrix is Singular, true = matrix inversion successful
*/ */
template<typename T> template<typename T>
static bool mat_inverse(const T* A, T* inv, uint8_t n) static bool mat_inverseN(const T* A, T* inv, uint16_t n)
{ {
T *L, *U, *P; T *L, *U, *P;
bool ret = true; bool ret = true;
@ -221,8 +216,8 @@ static bool mat_inverse(const T* A, T* inv, uint8_t n)
T *inv_pivoted = matrix_multiply(inv_unpivoted, P, n); T *inv_pivoted = matrix_multiply(inv_unpivoted, P, n);
//check sanity of results //check sanity of results
for(uint8_t i = 0; i < n; i++) { for(uint16_t i = 0; i < n; i++) {
for(uint8_t j = 0; j < n; j++) { for(uint16_t j = 0; j < n; j++) {
if(isnan(inv_pivoted[i*n+j]) || isinf(inv_pivoted[i*n+j])){ if(isnan(inv_pivoted[i*n+j]) || isinf(inv_pivoted[i*n+j])){
ret = false; ret = false;
} }
@ -247,7 +242,7 @@ static bool mat_inverse(const T* A, T* inv, uint8_t n)
* @returns false = matrix is Singular, true = matrix inversion successful * @returns false = matrix is Singular, true = matrix inversion successful
*/ */
template<typename T> template<typename T>
bool inverse3x3(const T m[], T invOut[]) static bool inverse3x3(const T m[], T invOut[])
{ {
T inv[9]; T inv[9];
// computes the inverse of a matrix m // computes the inverse of a matrix m
@ -270,7 +265,7 @@ bool inverse3x3(const T m[], T invOut[])
inv[7] = (m[6] * m[1] - m[0] * m[7]) * invdet; inv[7] = (m[6] * m[1] - m[0] * m[7]) * invdet;
inv[8] = (m[0] * m[4] - m[3] * m[1]) * invdet; inv[8] = (m[0] * m[4] - m[3] * m[1]) * invdet;
for(uint8_t i = 0; i < 9; i++){ for(uint16_t i = 0; i < 9; i++){
invOut[i] = inv[i]; invOut[i] = inv[i];
} }
@ -286,10 +281,10 @@ bool inverse3x3(const T m[], T invOut[])
* @returns false = matrix is Singular, true = matrix inversion successful * @returns false = matrix is Singular, true = matrix inversion successful
*/ */
template<typename T> template<typename T>
bool inverse4x4(const T m[],T invOut[]) static bool inverse4x4(const T m[],T invOut[])
{ {
T inv[16], det; T inv[16], det;
uint8_t i; uint16_t i;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
//disable FE_INEXACT detection as it fails on mac os runs //disable FE_INEXACT detection as it fails on mac os runs
@ -440,11 +435,41 @@ bool inverse4x4(const T m[],T invOut[])
* @returns false = matrix is Singular, true = matrix inversion successful * @returns false = matrix is Singular, true = matrix inversion successful
*/ */
template<typename T> template<typename T>
bool inverse(const T x[], T y[], uint16_t dim) bool mat_inverse(const T x[], T y[], uint16_t dim)
{ {
switch(dim){ switch(dim){
case 3: return inverse3x3(x,y); case 3: return inverse3x3(x,y);
case 4: return inverse4x4(x,y); case 4: return inverse4x4(x,y);
default: return mat_inverse(x,y,(uint8_t)dim); default: return mat_inverseN(x,y,dim);
} }
} }
template <typename T>
void mat_mul(const T *A, const T *B, T *C, uint16_t n)
{
memset(C, 0, sizeof(T)*n*n);
for(uint16_t i = 0; i < n; i++) {
for(uint16_t j = 0; j < n; j++) {
for(uint16_t k = 0;k < n; k++) {
C[i*n + j] += A[i*n + k] * B[k*n + j];
}
}
}
}
template <typename T>
void mat_identity(T *A, uint16_t n)
{
memset(A, 0, sizeof(T)*n*n);
for (uint16_t i=0; i<n; i++) {
A[i*n+i] = 1;
}
}
template bool mat_inverse<float>(const float x[], float y[], uint16_t dim);
template void mat_mul<float>(const float *A, const float *B, float *C, uint16_t n);
template void mat_identity<float>(float x[], uint16_t dim);
template bool mat_inverse<double>(const double x[], double y[], uint16_t dim);
template void mat_mul<double>(const double *A, const double *B, double *C, uint16_t n);
template void mat_identity<double>(double x[], uint16_t dim);