Line data    Source code

       1             : /*
       2             :   polynomial fitting class, originally written by Siddharth Bharat Purohit
       3             :   re-worked for ArduPilot by Andrew Tridgell
       4             : */
       5             : 
       6             : #include "polyfit.h"
       7             : #include "AP_Math.h"
       8             : #include "vector3.h"
       9             : 
      10             : template <uint8_t order, typename xtype, typename vtype>
      11         306 : void PolyFit<order,xtype,vtype>::update(xtype x, vtype y)
      12             : {
      13         306 :     xtype temp = 1;
      14             : 
      15        2448 :     for (int8_t i = 2*(order-1); i >= 0; i--) {
      16        2142 :         int8_t k = (i<order)?0:i - order + 1;
      17        7038 :         for (int8_t j = i - k; j >= k; j--) {
      18        4896 :             mat[j][i-j] += temp;
      19             :         }
      20        2142 :         temp *= x;
      21             :     }
      22             : 
      23         306 :     temp = 1;
      24        1530 :     for (int8_t i = order-1; i >= 0; i--) {
      25        1224 :         vec[i] += y * temp;
      26        1224 :         temp *= x;
      27             :     }
      28         306 : }
      29             : 
      30             : template <uint8_t order, typename xtype, typename vtype>
      31          48 : bool PolyFit<order,xtype,vtype>::get_polynomial(vtype res[order]) const
      32             : {
      33             :     // we dynamically allocate the inverse matrix to keep stack usage low
      34          48 :     xtype *inv_mat = NEW_NOTHROW xtype[order*order];
      35          48 :     if (inv_mat == nullptr) {
      36           0 :         return false;
      37             :     }
      38          48 :     if (!mat_inverse(&mat[0][0], inv_mat, order)) {
      39           0 :         delete[] inv_mat;
      40           0 :         return false;
      41             :     }
      42             :     // the summation must be done with double precision to get
      43             :     // good accuracy
      44          48 :     Vector3d resd[order] {};
      45         240 :     for (uint8_t i = 0; i < order; i++) {
      46         960 :         for (uint8_t j = 0; j < order; j++) {
      47         768 :             resd[i].x += vec[j].x * inv_mat[i*order+j];
      48         768 :             resd[i].y += vec[j].y * inv_mat[i*order+j];
      49         768 :             resd[i].z += vec[j].z * inv_mat[i*order+j];
      50             :         }
      51             :     }
      52         240 :     for (uint8_t j = 0; j < order; j++) {
      53         192 :         res[j].x = resd[j].x;
      54         192 :         res[j].y = resd[j].y;
      55         192 :         res[j].z = resd[j].z;
      56             :     }
      57          48 :     delete[] inv_mat;
      58          48 :     return true;
      59             : }
      60             : 
      61             : // instantiate for order 4 double with Vector3f
      62             : template class PolyFit<4, double, Vector3f>;