103 lines
2.6 KiB
C
103 lines
2.6 KiB
C
|
/*
|
||
|
|
* 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_ */
|