From a95f7ff8b013d48a3953d243fef0ef720604a7a4 Mon Sep 17 00:00:00 2001
From: Marius Eggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 19 Sep 2022 14:36:08 +0200
Subject: [PATCH] added CholeskyDecomposition

---
 .../acs/MultiplicativeKalmanFilter.cpp        |   4 +-
 .../acs/util/CholeskyDecomposition.h          | 102 ++++++++++++++++++
 2 files changed, 104 insertions(+), 2 deletions(-)
 create mode 100644 mission/controller/acs/util/CholeskyDecomposition.h

diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp
index 5d895566..5a546409 100644
--- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp
+++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp
@@ -12,8 +12,8 @@
 #include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
 #include <fsfw/src/fsfw/returnvalues/HasReturnvaluesIF.h>
 #include "MultiplicativeKalmanFilter.h"
-#include <acs/math/CholeskyDecomposition.h>
-#include <acs/math/MathOperations.h>
+#include <acs/util/CholeskyDecomposition.h>
+#include <acs/util/MathOperations.h>
 
 /*Initialisation of values for parameters in constructor*/
 MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_) :
diff --git a/mission/controller/acs/util/CholeskyDecomposition.h b/mission/controller/acs/util/CholeskyDecomposition.h
new file mode 100644
index 00000000..38ce0bcc
--- /dev/null
+++ b/mission/controller/acs/util/CholeskyDecomposition.h
@@ -0,0 +1,102 @@
+/*
+ * 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_ */