ardupilot/libraries/AP_Math/polyfit.cpp
2021-01-21 13:09:21 +11:00

47 lines
1.0 KiB
C++

/*
polynomial fitting class, originally written by Siddharth Bharat Purohit
re-worked for ArduPilot by Andrew Tridgell
*/
#include "polyfit.h"
#include "AP_Math.h"
template <uint8_t order>
void PolyFit<order>::update(float x, float y)
{
float temp = 1;
for (int8_t i = 2*(order-1); i >= 0; i--) {
int8_t k = (i<order)?0:i - order + 1;
for (int8_t j = i - k; j >= k; j--) {
mat[j][i-j] += temp;
}
temp *= x;
}
temp = 1;
for (int8_t i = order-1; i >= 0; i--) {
vec[i] += y * temp;
temp *= x;
}
}
template <uint8_t order>
bool PolyFit<order>::get_polynomial(float res[order]) const
{
float inv_mat[order][order];
if (!inverse(&mat[0][0], &inv_mat[0][0], order)) {
return false;
}
for (uint8_t i = 0; i < order; i++) {
res[i] = 0.0;
for (uint8_t j = 0; j < order; j++) {
res[i] += inv_mat[i][j] * vec[j];
}
}
return true;
}
// instantiate for order 4
template class PolyFit<4>;