Compass: Add math for 9 parameter fitting

libraries/AP_Compass/CompassCalibrator.cpp

@@ -511,6 +511,123 @@ void CompassCalibrator::run_ellipsoid_fit(uint8_t max_iterations)
 //////////////////////////////////////////////////////////
 ////////////////////// MATH HELPERS //////////////////////
 //////////////////////////////////////////////////////////
+bool CompassCalibrator::inverse9x9(const float x[81], float y[81])
+{
+  if(fabsf(det9x9(x)) < 1.0e-20f) {
+     return false;
+  }
+  float A[81];
+  int32_t i0;
+  int8_t ipiv[9];
+  int32_t j;
+  int32_t c;
+  int32_t pipk;
+  int32_t ix;
+  float smax;
+  int32_t k;
+  float s;
+  int32_t jy;
+  int32_t ijA;
+  int8_t p[9];
+  for (i0 = 0; i0 < 81; i0++) {
+    A[i0] = x[i0];
+    y[i0] = 0.0;
+  }
+
+  for (i0 = 0; i0 < 9; i0++) {
+    ipiv[i0] = (int8_t)(1 + i0);
+  }
+
+  for (j = 0; j < 8; j++) {
+    c = j * 10;
+    pipk = 0;
+    ix = c;
+    smax = fabs(A[c]);
+    for (k = 2; k <= 9 - j; k++) {
+      ix++;
+      s = fabs(A[ix]);
+      if (s > smax) {
+        pipk = k - 1;
+        smax = s;
+      }
+    }
+
+    if (A[c + pipk] != 0.0) {
+      if (pipk != 0) {
+        ipiv[j] = (int8_t)((j + pipk) + 1);
+        ix = j;
+        pipk += j;
+        for (k = 0; k < 9; k++) {
+          smax = A[ix];
+          A[ix] = A[pipk];
+          A[pipk] = smax;
+          ix += 9;
+          pipk += 9;
+        }
+      }
+
+      i0 = (c - j) + 9;
+      for (jy = c + 1; jy + 1 <= i0; jy++) {
+        A[jy] /= A[c];
+      }
+    }
+
+    pipk = c;
+    jy = c + 9;
+    for (k = 1; k <= 8 - j; k++) {
+      smax = A[jy];
+      if (A[jy] != 0.0) {
+        ix = c + 1;
+        i0 = (pipk - j) + 18;
+        for (ijA = 10 + pipk; ijA + 1 <= i0; ijA++) {
+          A[ijA] += A[ix] * -smax;
+          ix++;
+        }
+      }
+
+      jy += 9;
+      pipk += 9;
+    }
+  }
+
+  for (i0 = 0; i0 < 9; i0++) {
+    p[i0] = (int8_t)(1 + i0);
+  }
+
+  for (k = 0; k < 8; k++) {
+    if (ipiv[k] > 1 + k) {
+      pipk = p[ipiv[k] - 1];
+      p[ipiv[k] - 1] = p[k];
+      p[k] = (int8_t)pipk;
+    }
+  }
+
+  for (k = 0; k < 9; k++) {
+    y[k + 9 * (p[k] - 1)] = 1.0;
+    for (j = k; j + 1 < 10; j++) {
+      if (y[j + 9 * (p[k] - 1)] != 0.0) {
+        for (jy = j + 1; jy + 1 < 10; jy++) {
+          y[jy + 9 * (p[k] - 1)] -= y[j + 9 * (p[k] - 1)] * A[jy + 9 * j];
+        }
+      }
+    }
+  }
+
+  for (j = 0; j < 9; j++) {
+    c = 9 * j;
+    for (k = 8; k > -1; k += -1) {
+      pipk = 9 * k;
+      if (y[k + c] != 0.0) {
+        y[k + c] /= A[k + pipk];
+        for (jy = 0; jy + 1 <= k; jy++) {
+          y[jy + c] -= y[k + c] * A[jy + pipk];
+        }
+      }
+    }
+  }
+  return true;
+}
+
 
 bool CompassCalibrator::inverse6x6(const float x[], float y[])
 {
@@ -892,6 +1009,95 @@ float CompassCalibrator::det6x6(const float C[36])
     return f;
 }
 
+float CompassCalibrator::det9x9(const float C[81])
+{
+  float f;
+  float A[81];
+  int8_t ipiv[9];
+  int32_t i0;
+  int32_t j;
+  int32_t c;
+  int32_t iy;
+  int32_t ix;
+  float smax;
+  int32_t jy;
+  float s;
+  int32_t b_j;
+  int32_t ijA;
+  bool isodd;
+  memcpy(&A[0], &C[0], 81U * sizeof(float));
+  for (i0 = 0; i0 < 9; i0++) {
+    ipiv[i0] = (int8_t)(1 + i0);
+  }
+
+  for (j = 0; j < 8; j++) {
+    c = j * 10;
+    iy = 0;
+    ix = c;
+    smax = fabs(A[c]);
+    for (jy = 2; jy <= 9 - j; jy++) {
+      ix++;
+      s = fabs(A[ix]);
+      if (s > smax) {
+        iy = jy - 1;
+        smax = s;
+      }
+    }
+
+    if (A[c + iy] != 0.0) {
+      if (iy != 0) {
+        ipiv[j] = (int8_t)((j + iy) + 1);
+        ix = j;
+        iy += j;
+        for (jy = 0; jy < 9; jy++) {
+          smax = A[ix];
+          A[ix] = A[iy];
+          A[iy] = smax;
+          ix += 9;
+          iy += 9;
+        }
+      }
+
+      i0 = (c - j) + 9;
+      for (iy = c + 1; iy + 1 <= i0; iy++) {
+        A[iy] /= A[c];
+      }
+    }
+
+    iy = c;
+    jy = c + 9;
+    for (b_j = 1; b_j <= 8 - j; b_j++) {
+      smax = A[jy];
+      if (A[jy] != 0.0) {
+        ix = c + 1;
+        i0 = (iy - j) + 18;
+        for (ijA = 10 + iy; ijA + 1 <= i0; ijA++) {
+          A[ijA] += A[ix] * -smax;
+          ix++;
+        }
+      }
+
+      jy += 9;
+      iy += 9;
+    }
+  }
+
+  f = A[0];
+  isodd = FALSE;
+  for (jy = 0; jy < 8; jy++) {
+    f *= A[(jy + 9 * (1 + jy)) + 1];
+    if (ipiv[jy] > 1 + jy) {
+      isodd = !isodd;
+    }
+  }
+
+  if (isodd) {
+    f = -f;
+  }
+
+  return f;
+}
+
 uint16_t CompassCalibrator::get_random(void)
 {
     static uint32_t m_z = 1234;

libraries/AP_Compass/CompassCalibrator.h

@@ -118,8 +118,11 @@ private:
 
     sphere_param_t _sphere_param;
     ellipsoid_param_t _ellipsoid_param;
-
+    bool _running_ellipsoid_fit:1;
     // math helpers
+    
+    bool inverse9x9(const float m[],float invOut[]);
+    float det9x9(const float m[]);
     bool inverse6x6(const float m[],float invOut[]);
     float det6x6(const float m[]);
     bool inverse4x4(float m[],float invOut[]);