mission/controller/acs/util/CholeskyDecomposition.h

@@ -1,98 +0,0 @@
-/*
- * TinyEKF: Extended Kalman Filter for embedded processors
- *
- * Copyright (C) 2015 Simon D. Levy
- *
- * MIT License
- */
-#ifndef CHOLESKYDECOMPOSITION_H_
-#define CHOLESKYDECOMPOSITION_H_
-#include <math.h>
-// typedef unsigned int uint8_t;
-
-template <typename T1, typename T2 = T1, typename T3 = T2>
-class CholeskyDecomposition {
- public:
-  static int invertCholesky(T1 *matrix, T2 *result, T3 *tempMatrix, const uint8_t dimension) {
-    // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
-    return cholsl(matrix, result, tempMatrix, dimension);
-  }
-
- private:
-  // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
-  static uint8_t choldc1(double *a, double *p, uint8_t n) {
-    int8_t i, j, k;
-    double sum;
-
-    for (i = 0; i < n; i++) {
-      for (j = i; j < n; j++) {
-        sum = a[i * n + j];
-        for (k = i - 1; k >= 0; k--) {
-          sum -= a[i * n + k] * a[j * n + k];
-        }
-        if (i == j) {
-          if (sum <= 0) {
-            return 1; /* error */
-          }
-          p[i] = sqrt(sum);
-        } else {
-          a[j * n + i] = sum / p[i];
-        }
-      }
-    }
-
-    return 0; /* success */
-  }
-
-  // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
-  static uint8_t choldcsl(double *A, double *a, double *p, uint8_t n) {
-    uint8_t i, j, k;
-    double sum;
-    for (i = 0; i < n; i++)
-      for (j = 0; j < n; j++) a[i * n + j] = A[i * n + j];
-    if (choldc1(a, p, n)) return 1;
-    for (i = 0; i < n; i++) {
-      a[i * n + i] = 1 / p[i];
-      for (j = i + 1; j < n; j++) {
-        sum = 0;
-        for (k = i; k < j; k++) {
-          sum -= a[j * n + k] * a[k * n + i];
-        }
-        a[j * n + i] = sum / p[j];
-      }
-    }
-
-    return 0; /* success */
-  }
-
-  // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
-  static uint8_t cholsl(double *A, double *a, double *p, uint8_t n) {
-    uint8_t i, j, k;
-    if (choldcsl(A, a, p, n)) return 1;
-    for (i = 0; i < n; i++) {
-      for (j = i + 1; j < n; j++) {
-        a[i * n + j] = 0.0;
-      }
-    }
-    for (i = 0; i < n; i++) {
-      a[i * n + i] *= a[i * n + i];
-      for (k = i + 1; k < n; k++) {
-        a[i * n + i] += a[k * n + i] * a[k * n + i];
-      }
-      for (j = i + 1; j < n; j++) {
-        for (k = j; k < n; k++) {
-          a[i * n + j] += a[k * n + i] * a[k * n + j];
-        }
-      }
-    }
-    for (i = 0; i < n; i++) {
-      for (j = 0; j < i; j++) {
-        a[i * n + j] = a[j * n + i];
-      }
-    }
-
-    return 0; /* success */
-  }
-};
-
-#endif /* CONTRIB_MATH_CHOLESKYDECOMPOSITION_H_ */