AP_Math: added polygon fitting code

libraries/AP_Math/polyfit.cpp

@@ -0,0 +1,46 @@
+/*
+  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>;

libraries/AP_Math/polyfit.h

@@ -0,0 +1,23 @@
+/*
+  polynomial fitting class, originally written by Siddharth Bharat Purohit
+  re-written for ArduPilot by Andrew Tridgell
+
+  This fits a polynomial of a given order to a set of data, running as
+  an online algorithm with minimal storage
+*/
+
+#pragma once
+
+#include <stdint.h>
+
+template <uint8_t order>
+class PolyFit {
+public:
+    void update(float x, float y);
+    bool get_polynomial(float res[order]) const;
+
+private:
+    float mat[order][order];
+    float vec[order];
+};
+