diff --git a/libraries/AP_Math/polyfit.cpp b/libraries/AP_Math/polyfit.cpp
index 15bd9d2228..3a70267185 100644
--- a/libraries/AP_Math/polyfit.cpp
+++ b/libraries/AP_Math/polyfit.cpp
@@ -5,11 +5,12 @@
 
 #include "polyfit.h"
 #include "AP_Math.h"
+#include "vector3.h"
 
-template <uint8_t order>
-void PolyFit<order>::update(float x, float y)
+template <uint8_t order, typename xtype, typename vtype>
+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--) {
         int8_t k = (i<order)?0:i - order + 1;
@@ -18,7 +19,7 @@ void PolyFit<order>::update(float x, float y)
         }
         temp *= x;
     }
-    
+
     temp = 1;
     for (int8_t i = order-1; i >= 0; i--) {
         vec[i] += y * temp;
@@ -26,21 +27,36 @@ void PolyFit<order>::update(float x, float y)
     }
 }
 
-template <uint8_t order>
-bool PolyFit<order>::get_polynomial(float res[order]) const
+template <uint8_t order, typename xtype, typename vtype>
+bool PolyFit<order,xtype,vtype>::get_polynomial(vtype res[order]) const
 {
-    double inv_mat[order][order];
-    if (!inverse(&mat[0][0], &inv_mat[0][0], order)) {
+    // we dynamically allocate the inverse matrix to keep stack usage low
+    xtype *inv_mat = new xtype[order*order];
+    if (inv_mat == nullptr) {
         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++) {
-        res[i] = 0.0;
         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;
 }
 
-// instantiate for order 4
-template class PolyFit<4>;
+// instantiate for order 4 double with Vector3f
+template class PolyFit<4, double, Vector3f>;
diff --git a/libraries/AP_Math/polyfit.h b/libraries/AP_Math/polyfit.h
index 3797609427..10a4227ec3 100644
--- a/libraries/AP_Math/polyfit.h
+++ b/libraries/AP_Math/polyfit.h
@@ -10,14 +10,18 @@
 
 #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:
-    void update(float x, float y);
-    bool get_polynomial(float res[order]) const;
+    void update(xtype x, vtype y);
+    bool get_polynomial(vtype res[order]) const;
 
 private:
-    double mat[order][order];
-    double vec[order];
+    xtype mat[order][order];
+    vtype vec[order];
 };