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

AP_Math: templatize matrix_alg methods

Browse Source
This commit is contained in:
bugobliterator 2021-01-09 23:39:13 +05:30 committed by Peter Barker
parent 6254787c26
commit 3b3e2c01f8
4 changed files with 69 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_Math/AP_Math.h
View File

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

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

@ -16,7 +16,7 @@
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma GCC optimize("O2")
#pragma once
#include <AP_HAL/AP_HAL.h>
#include <stdio.h>
@ -24,9 +24,8 @@
#include <fenv.h>
#endif
#include <AP_Math/AP_Math.h>
extern const AP_HAL::HAL& hal;
// extern const AP_HAL::HAL& hal;
//TODO: use higher precision datatypes to achieve more accuracy for matrix algebra operations
@ -38,11 +37,11 @@ extern const AP_HAL::HAL& hal;
* @param n, dimemsion of square matrices
* @returns multiplied matrix i.e. A*B
*/
float* mat_mul(const float *A, const float *B, uint8_t n)
template<typename T>
T* matrix_multiply(const T *A, const T *B, uint8_t n)
{
float* ret = new float[n*n];
memset(ret,0.0f,n*n*sizeof(float));
T* ret = new T[n*n];
memset(ret,0.0f,n*n*sizeof(T));
for(uint8_t i = 0; i < n; i++) {
for(uint8_t j = 0; j < n; j++) {
@ -54,9 +53,10 @@ float* mat_mul(const float *A, const float *B, uint8_t n)
return ret;
}
static inline void swap(float &a, float &b)
template<typename T>
static inline void swap(T &a, T &b)
{
float c;
T c;
c = a;
a = b;
b = c;
@ -70,20 +70,26 @@ static inline void swap(float &a, float &b)
* @param n, dimenstion of square matrix
* @returns false = matrix is Singular or non positive definite, true = matrix inversion successful
*/
static void mat_pivot(const float* A, float* pivot, uint8_t n)
template<typename T>
static void mat_pivot(const T* A, T* pivot, uint8_t n)
{
for(uint8_t i = 0;i<n;i++){
for(uint8_t j=0;j<n;j++) {
pivot[i*n+j] = static_cast<float>(i==j);
pivot[i*n+j] = static_cast<T>(i==j);
}
}
for(uint8_t i = 0;i < n; i++) {
uint8_t max_j = i;
for(uint8_t j=i;j<n;j++){
if(fabsf(A[j*n + i]) > fabsf(A[max_j*n + i])) {
max_j = j;
if (std::is_same<T, double>::value) {
if(fabs(A[j*n + i]) > fabs(A[max_j*n + i])) {
max_j = j;
}
} else {
if(fabsf(A[j*n + i]) > fabsf(A[max_j*n + i])) {
max_j = j;
}
}
}
@ -102,8 +108,8 @@ static void mat_pivot(const float* A, float* pivot, uint8_t n)
* @param out, Output inverted lower triangular matrix
* @param n, dimension of matrix
*/
static void mat_forward_sub(const float *L, float *out, uint8_t n)
template<typename T>
static void mat_forward_sub(const T *L, T *out, uint8_t n)
{
// Forward substitution solve LY = I
for(int i = 0; i < n; i++) {
@ -124,8 +130,8 @@ static void mat_forward_sub(const float *L, float *out, uint8_t n)
* @param out, Output inverted upper triangular matrix
* @param n, dimension of matrix
*/
static void mat_back_sub(const float *U, float *out, uint8_t n)
template<typename T>
static void mat_back_sub(const T *U, T *out, uint8_t n)
{
// Backward Substitution solve UY = I
for(int i = n-1; i >= 0; i--) {
@ -147,15 +153,15 @@ static void mat_back_sub(const float *U, float *out, uint8_t n)
* @param out, Output inverted upper triangular matrix
* @param n, dimension of matrix
*/
static void mat_LU_decompose(const float* A, float* L, float* U, float *P, uint8_t n)
template<typename T>
static void mat_LU_decompose(const T* A, T* L, T* U, T *P, uint8_t n)
{
memset(L,0,n*n*sizeof(float));
memset(U,0,n*n*sizeof(float));
memset(P,0,n*n*sizeof(float));
memset(L,0,n*n*sizeof(T));
memset(U,0,n*n*sizeof(T));
memset(P,0,n*n*sizeof(T));
mat_pivot(A,P,n);
float *APrime = mat_mul(P,A,n);
T *APrime = matrix_multiply(P,A,n);
for(uint8_t i = 0; i < n; i++) {
L[i*n + i] = 1;
}
@ -188,30 +194,31 @@ static void mat_LU_decompose(const float* A, float* L, float* U, float *P, uint8
* @param n, dimension of square matrix
* @returns false = matrix is Singular, true = matrix inversion successful
*/
static bool mat_inverse(const float* A, float* inv, uint8_t n)
template<typename T>
static bool mat_inverse(const T* A, T* inv, uint8_t n)
{
float *L, *U, *P;
T *L, *U, *P;
bool ret = true;
L = new float[n*n];
U = new float[n*n];
P = new float[n*n];
L = new T[n*n];
U = new T[n*n];
P = new T[n*n];
mat_LU_decompose(A,L,U,P,n);
float *L_inv = new float[n*n];
float *U_inv = new float[n*n];
T *L_inv = new T[n*n];
T *U_inv = new T[n*n];
memset(L_inv,0,n*n*sizeof(float));
memset(L_inv,0,n*n*sizeof(T));
mat_forward_sub(L,L_inv,n);
memset(U_inv,0,n*n*sizeof(float));
memset(U_inv,0,n*n*sizeof(T));
mat_back_sub(U,U_inv,n);
// decomposed matrices no longer required
delete[] L;
delete[] U;
float *inv_unpivoted = mat_mul(U_inv,L_inv,n);
float *inv_pivoted = mat_mul(inv_unpivoted, P, n);
T *inv_unpivoted = matrix_multiply(U_inv,L_inv,n);
T *inv_pivoted = matrix_multiply(inv_unpivoted, P, n);
//check sanity of results
for(uint8_t i = 0; i < n; i++) {
@ -221,7 +228,7 @@ static bool mat_inverse(const float* A, float* inv, uint8_t n)
}
}
}
memcpy(inv,inv_pivoted,n*n*sizeof(float));
memcpy(inv,inv_pivoted,n*n*sizeof(T));
//free memory
delete[] inv_pivoted;
@ -239,19 +246,19 @@ static bool mat_inverse(const float* A, float* inv, uint8_t n)
* @param invOut, Output inverted 4x4 matrix
* @returns false = matrix is Singular, true = matrix inversion successful
*/
bool inverse3x3(const float m[], float invOut[])
template<typename T>
bool inverse3x3(const T m[], T invOut[])
{
float inv[9];
T inv[9];
// computes the inverse of a matrix m
float det = m[0] * (m[4] * m[8] - m[7] * m[5]) -
T det = m[0] * (m[4] * m[8] - m[7] * m[5]) -
m[1] * (m[3] * m[8] - m[5] * m[6]) +
m[2] * (m[3] * m[7] - m[4] * m[6]);
if (is_zero(det) || isinf(det)) {
return false;
}
float invdet = 1 / det;
T invdet = 1 / det;
inv[0] = (m[4] * m[8] - m[7] * m[5]) * invdet;
inv[1] = (m[2] * m[7] - m[1] * m[8]) * invdet;
@ -278,17 +285,17 @@ bool inverse3x3(const float m[], float invOut[])
* @param invOut, Output inverted 4x4 matrix
* @returns false = matrix is Singular, true = matrix inversion successful
*/
bool inverse4x4(const float m[],float invOut[])
template<typename T>
bool inverse4x4(const T m[],T invOut[])
{
float inv[16], det;
T inv[16], det;
uint8_t i;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
//disable FE_INEXACT detection as it fails on mac os runs
int old = fedisableexcept(FE_INEXACT | FE_OVERFLOW);
if (old < 0) {
hal.console->printf("inverse4x4(): warning: error on disabling FE_OVERFLOW floating point exception\n");
// hal.console->printf("inverse4x4(): warning: error on disabling FE_OVERFLOW floating point exception\n");
}
#endif
@ -417,7 +424,7 @@ bool inverse4x4(const float m[],float invOut[])
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (old >= 0 && feenableexcept(old) < 0) {
hal.console->printf("inverse4x4(): warning: error on restoring floating exception mask\n");
// hal.console->printf("inverse4x4(): warning: error on restoring floating exception mask\n");
}
#endif
@ -432,11 +439,12 @@ bool inverse4x4(const float m[],float invOut[])
* @param n, dimension of square matrix
* @returns false = matrix is Singular, true = matrix inversion successful
*/
bool inverse(const float x[], float y[], uint16_t dim)
template<typename T>
bool inverse(const T x[], T y[], uint16_t dim)
{
switch(dim){
case 3: return inverse3x3(x,y);
case 4: return inverse4x4(x,y);
default: return mat_inverse(x,y,dim);
default: return mat_inverse(x,y,(uint8_t)dim);
}
}

4
libraries/AP_Math/polyfit.cpp
View File

@ -9,7 +9,7 @@
template <uint8_t order>
void PolyFit<order>::update(float x, float y)
{
float temp = 1;
double temp = 1;
for (int8_t i = 2*(order-1); i >= 0; i--) {
int8_t k = (i<order)?0:i - order + 1;
@ -29,7 +29,7 @@ void PolyFit<order>::update(float x, float y)
template <uint8_t order>
bool PolyFit<order>::get_polynomial(float res[order]) const
{
float inv_mat[order][order];
double inv_mat[order][order];
if (!inverse(&mat[0][0], &inv_mat[0][0], order)) {
return false;
}

4
libraries/AP_Math/polyfit.h
View File

@ -17,7 +17,7 @@ public:
bool get_polynomial(float res[order]) const;
private:
float mat[order][order];
float vec[order];
double mat[order][order];
double vec[order];
};