eive/eive-obsw
eive
/
eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests 1 Releases 117 Wiki Activity

never needed this anyways

This commit is contained in:
Marius Eggert 2024-01-19 14:18:12 +01:00
parent 13f3739386
commit 883ff4f6de
1 changed files with 0 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

98
mission/controller/acs/util/CholeskyDecomposition.h
View File

@ -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_ */