mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 15:08:28 -04:00
47 lines
1.0 KiB
C++
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>;
|