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

AP_Math: specialise polyfit for vectors

Browse Source 
this is needed to get the precision good enough for wide temperature
ranges when summing the resulting polygon coefficients
This commit is contained in:
Andrew Tridgell 2021-01-18 12:28:32 +11:00 committed by Peter Barker
parent c8aaceb6ac
commit a1fb0592a9
2 changed files with 38 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
libraries/AP_Math/polyfit.cpp
View File

@ -5,11 +5,12 @@
#include "polyfit.h" #include "polyfit.h"
#include "AP_Math.h" #include "AP_Math.h"
#include "vector3.h"
template <uint8_t order> template <uint8_t order, typename xtype, typename vtype>
void PolyFit<order>::update(float x, float y) void PolyFit<order,xtype,vtype>::update(xtype x, vtype y)
{ {
double temp = 1; xtype temp = 1;
for (int8_t i = 2*(order-1); i >= 0; i--) { for (int8_t i = 2*(order-1); i >= 0; i--) {
int8_t k = (i<order)?0:i - order + 1; int8_t k = (i<order)?0:i - order + 1;
@ -18,7 +19,7 @@ void PolyFit<order>::update(float x, float y)
} }
temp *= x; temp *= x;
} }
temp = 1; temp = 1;
for (int8_t i = order-1; i >= 0; i--) { for (int8_t i = order-1; i >= 0; i--) {
vec[i] += y * temp; vec[i] += y * temp;
@ -26,21 +27,36 @@ void PolyFit<order>::update(float x, float y)
} }
} }
template <uint8_t order> template <uint8_t order, typename xtype, typename vtype>
bool PolyFit<order>::get_polynomial(float res[order]) const bool PolyFit<order,xtype,vtype>::get_polynomial(vtype res[order]) const
{ {
double inv_mat[order][order]; // we dynamically allocate the inverse matrix to keep stack usage low
if (!inverse(&mat[0][0], &inv_mat[0][0], order)) { xtype *inv_mat = new xtype[order*order];
if (inv_mat == nullptr) {
return false; return false;
} }
if (!mat_inverse(&mat[0][0], inv_mat, order)) {
delete[] inv_mat;
return false;
}
// the summation must be done with double precision to get
// good accuracy
Vector3d resd[order] {};
for (uint8_t i = 0; i < order; i++) { for (uint8_t i = 0; i < order; i++) {
res[i] = 0.0;
for (uint8_t j = 0; j < order; j++) { for (uint8_t j = 0; j < order; j++) {
res[i] += inv_mat[i][j] * vec[j]; resd[i].x += vec[j].x * inv_mat[i*order+j];
resd[i].y += vec[j].y * inv_mat[i*order+j];
resd[i].z += vec[j].z * inv_mat[i*order+j];
} }
} }
for (uint8_t j = 0; j < order; j++) {
res[j].x = resd[j].x;
res[j].y = resd[j].y;
res[j].z = resd[j].z;
}
delete[] inv_mat;
return true; return true;
} }
// instantiate for order 4 // instantiate for order 4 double with Vector3f
template class PolyFit<4>; template class PolyFit<4, double, Vector3f>;

16
libraries/AP_Math/polyfit.h
View File

@ -10,14 +10,18 @@
#include <stdint.h> #include <stdint.h>
template <uint8_t order> /*
class PolyFit { polynomial fit with X axis type xtype and yaxis type vtype (must be a vector)
*/
template <uint8_t order, typename xtype, typename vtype>
class PolyFit
{
public: public:
void update(float x, float y); void update(xtype x, vtype y);
bool get_polynomial(float res[order]) const; bool get_polynomial(vtype res[order]) const;
private: private:
double mat[order][order]; xtype mat[order][order];
double vec[order]; vtype vec[order];
}; };