diff --git a/mission/controller/acs/util/CholeskyDecomposition.h b/mission/controller/acs/util/CholeskyDecomposition.h deleted file mode 100644 index 667f9f63..00000000 --- a/mission/controller/acs/util/CholeskyDecomposition.h +++ /dev/null @@ -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 -// typedef unsigned int uint8_t; - -template -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_ */