eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

afmt
Some checks failed
EIVE/eive-obsw/pipeline/pr-develop There was a failure building this commit
Details

Browse Source
This commit is contained in:
Robin Müller 2022-12-01 15:56:55 +01:00
parent a3a919437e
commit 828738ba0e
No known key found for this signature in database
GPG Key ID: 11D4952C8CCEF814
14 changed files with 217 additions and 240 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
dummies/GyroL3GD20Dummy.cpp
View File

@ -38,7 +38,6 @@ void GyroL3GD20Dummy::fillCommandAndReplyMap() {}
uint32_t GyroL3GD20Dummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 500; }
ReturnValue_t GyroL3GD20Dummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X, new PoolEntry<float>({1.2}, true));

2
dummies/ImtqDummy.cpp
View File

@ -39,7 +39,7 @@ uint32_t ImtqDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { retur
ReturnValue_t ImtqDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, new PoolEntry<float>({0.0,0.0,0.0}));
localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, new PoolEntry<float>({0.0, 0.0, 0.0}));
localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(IMTQ::MTM_RAW, new PoolEntry<float>({0.12, 0.76, -0.45}, true));
localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry<uint8_t>({0}));

8
dummies/SusDummy.cpp
View File

@ -35,9 +35,9 @@ uint32_t SusDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return
ReturnValue_t SusDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
localDataPoolMap.emplace(SUS::SusPoolIds::TEMPERATURE_C, new PoolEntry<float>({0}, 1, true));
localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC,
new PoolEntry<uint16_t>({0, 0, 0, 0, 0, 0},true));
localDataPoolMap.emplace(SUS::SusPoolIds::TEMPERATURE_C, new PoolEntry<float>({0}, 1, true));
localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC,
new PoolEntry<uint16_t>({0, 0, 0, 0, 0, 0}, true));
return returnvalue::OK;
return returnvalue::OK;
}

2
mission/controller/AcsController.h
View File

@ -70,7 +70,7 @@ class AcsController : public ExtendedControllerBase {
void announceMode(bool recursive);
/* ACS Datasets */
IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER);
IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER);
// MGMs
acsctrl::MgmDataRaw mgmDataRaw;
PoolEntry<float> mgm0VecRaw = PoolEntry<float>(3);

2
mission/controller/CMakeLists.txt
View File

@ -3,4 +3,4 @@ if(TGT_BSP MATCHES "arm/q7s" OR TGT_BSP MATCHES "")
AcsController.cpp)
endif()
add_subdirectory(acs)
add_subdirectory(acs)

18
mission/controller/acs/CMakeLists.txt
View File

@ -1,13 +1,13 @@
target_sources(
${LIB_EIVE_MISSION}
PRIVATE AcsParameters.cpp
ActuatorCmd.cpp
Guidance.cpp
Igrf13Model.cpp
MultiplicativeKalmanFilter.cpp
Navigation.cpp
SensorProcessing.cpp
SensorValues.cpp
SusConverter.cpp)
ActuatorCmd.cpp
Guidance.cpp
Igrf13Model.cpp
MultiplicativeKalmanFilter.cpp
Navigation.cpp
SensorProcessing.cpp
SensorValues.cpp
SusConverter.cpp)
add_subdirectory(control)

10
mission/controller/acs/SusConverter.cpp
View File

@ -108,14 +108,14 @@ void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffB
float* SusConverter::calculateSunVector() {
// Calculate the normalized Sun Vector
sunVectorSensorFrame[0] = -(tan(alphaBetaCalibrated[0] * (M_PI / 180)) /
(sqrt((powf(tan(alphaBetaCalibrated[0] * (M_PI / 180)), 2)) +
powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
(sqrt((powf(tan(alphaBetaCalibrated[0] * (M_PI / 180)), 2)) +
powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
sunVectorSensorFrame[1] = -(tan(alphaBetaCalibrated[1] * (M_PI / 180)) /
(sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) +
powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
(sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) +
powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
sunVectorSensorFrame[2] =
-(-1 / (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) +
powf((tan(alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
powf((tan(alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
return sunVectorSensorFrame;
}

8
mission/controller/acs/SusConverter.h
View File

@ -27,10 +27,10 @@ class SusConverter {
const float coeffBeta[9][10]);
private:
float alphaBetaRaw[2]; //[°]
// float coeffAlpha[9][10];
// float coeffBeta[9][10];
float alphaBetaCalibrated[2]; //[°]
float alphaBetaRaw[2]; //[°]
// float coeffAlpha[9][10];
// float coeffBeta[9][10];
float alphaBetaCalibrated[2]; //[°]
float sunVectorSensorFrame[3]; //[-]
bool validFlag[12] = {returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK,

15
mission/controller/acs/config/classIds.h
View File

@ -5,15 +5,14 @@
#include <fsfw/returnvalues/FwClassIds.h>
namespace CLASS_ID {
enum eiveclassIds: uint8_t {
EIVE_CLASS_ID_START = COMMON_CLASS_ID_END,
KALMAN,
SAFE,
PTG,
DETUMBLE,
EIVE_CLASS_ID_END // [EXPORT] : [END]
enum eiveclassIds : uint8_t {
EIVE_CLASS_ID_START = COMMON_CLASS_ID_END,
KALMAN,
SAFE,
PTG,
DETUMBLE,
EIVE_CLASS_ID_END // [EXPORT] : [END]
};
}
#endif /* ACS_CONFIG_CLASSIDS_H_ */

7
mission/controller/acs/control/CMakeLists.txt
View File

@ -1,5 +1,2 @@
target_sources(
${LIB_EIVE_MISSION}
PRIVATE Detumble.cpp
PtgCtrl.cpp
SafeCtrl.cpp)
target_sources(${LIB_EIVE_MISSION} PRIVATE Detumble.cpp PtgCtrl.cpp
SafeCtrl.cpp)

22
mission/controller/acs/control/Detumble.cpp
View File

@ -6,30 +6,24 @@
* Author: Robin Marquardt
*/
#include "Detumble.h"
#include "../util/MathOperations.h"
#include <math.h>
#include <fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/globalfunctions/sign.h>
#include <math.h>
#include "../util/MathOperations.h"
Detumble::Detumble(AcsParameters *acsParameters_){
loadAcsParameters(acsParameters_);
}
Detumble::Detumble(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); }
Detumble::~Detumble(){
}
void Detumble::loadAcsParameters(AcsParameters *acsParameters_){
detumbleCtrlParameters = &(acsParameters_->detumbleCtrlParameters);
magnetorquesParameter = &(acsParameters_->magnetorquesParameter);
Detumble::~Detumble() {}
void Detumble::loadAcsParameters(AcsParameters *acsParameters_) {
detumbleCtrlParameters = &(acsParameters_->detumbleCtrlParameters);
magnetorquesParameter = &(acsParameters_->magnetorquesParameter);
}
ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid,

46
mission/controller/acs/control/Detumble.h
View File

@ -8,38 +8,36 @@
#ifndef ACS_CONTROL_DETUMBLE_H_
#define ACS_CONTROL_DETUMBLE_H_
#include "../SensorValues.h"
#include "../AcsParameters.h"
#include "../config/classIds.h"
#include <string.h>
#include <stdio.h>
#include <time.h>
#include <fsfw/returnvalues/returnvalue.h>
#include <stdio.h>
#include <string.h>
#include <time.h>
#include "../AcsParameters.h"
#include "../SensorValues.h"
#include "../config/classIds.h"
class Detumble{
class Detumble {
public:
Detumble(AcsParameters *acsParameters_);
virtual ~Detumble();
public:
Detumble(AcsParameters *acsParameters_);
virtual ~Detumble();
static const uint8_t INTERFACE_ID = CLASS_ID::DETUMBLE;
static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01);
static const uint8_t INTERFACE_ID = CLASS_ID::DETUMBLE;
static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01);
/* @brief: Load AcsParameters für this class
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/
void loadAcsParameters(AcsParameters *acsParameters_);
/* @brief: Load AcsParameters für this class
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/
void loadAcsParameters(AcsParameters *acsParameters_);
ReturnValue_t bDotLaw(const double *magRate, const bool magRateValid, const double *magField,
const bool magFieldValid, double *magMom);
ReturnValue_t bDotLaw(const double *magRate, const bool magRateValid,
const double *magField, const bool magFieldValid, double *magMom);
ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom);
ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom);
private:
AcsParameters::DetumbleCtrlParameters* detumbleCtrlParameters;
AcsParameters::MagnetorquesParameter* magnetorquesParameter;
private:
AcsParameters::DetumbleCtrlParameters *detumbleCtrlParameters;
AcsParameters::MagnetorquesParameter *magnetorquesParameter;
};
#endif /*ACS_CONTROL_DETUMBLE_H_*/

160
mission/controller/acs/control/PtgCtrl.cpp
View File

@ -5,10 +5,8 @@
* Author: Robin Marquardt
*/
#include "PtgCtrl.h"
#include "../util/MathOperations.h"
#include <fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
@ -16,100 +14,96 @@
#include <fsfw/globalfunctions/sign.h>
#include <math.h>
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_){
loadAcsParameters(acsParameters_);
#include "../util/MathOperations.h"
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); }
PtgCtrl::~PtgCtrl() {}
void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters);
inertiaEIVE = &(acsParameters_->inertiaEIVE);
rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
rwMatrices = &(acsParameters_->rwMatrices);
}
PtgCtrl::~PtgCtrl(){
void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const double *deltaRate,
const double *rwPseudoInv, double *torqueRws) {
//------------------------------------------------------------------------------------------------
// Compute gain matrix K and P matrix
//------------------------------------------------------------------------------------------------
double om = pointingModeControllerParameters->om;
double zeta = pointingModeControllerParameters->zeta;
double qErrorMin = pointingModeControllerParameters->qiMin;
double omMax = pointingModeControllerParameters->omMax;
}
double cInt = 2 * om * zeta;
double kInt = 2 * pow(om, 2);
void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_){
pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters);
inertiaEIVE = &(acsParameters_->inertiaEIVE);
rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
rwMatrices =&(acsParameters_->rwMatrices);
}
double qErrorLaw[3] = {0, 0, 0};
void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const double *deltaRate,const double *rwPseudoInv, double *torqueRws){
for (int i = 0; i < 3; i++) {
if (abs(qError[i]) < qErrorMin) {
qErrorLaw[i] = qErrorMin;
} else {
qErrorLaw[i] = abs(qError[i]);
}
}
double qErrorLawNorm = VectorOperations<double>::norm(qErrorLaw, 3);
//------------------------------------------------------------------------------------------------
// Compute gain matrix K and P matrix
//------------------------------------------------------------------------------------------------
double om = pointingModeControllerParameters->om;
double zeta = pointingModeControllerParameters->zeta;
double qErrorMin = pointingModeControllerParameters->qiMin;
double omMax = pointingModeControllerParameters->omMax;
double gain1 = cInt * omMax / qErrorLawNorm;
double gainVector[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(qErrorLaw, gain1, gainVector, 3);
double cInt = 2 * om * zeta;
double kInt = 2 * pow(om,2);
double gainMatrixDiagonal[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double gainMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
gainMatrixDiagonal[0][0] = gainVector[0];
gainMatrixDiagonal[1][1] = gainVector[1];
gainMatrixDiagonal[2][2] = gainVector[2];
MatrixOperations<double>::multiply(*gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix),
*gainMatrix, 3, 3, 3);
double qErrorLaw[3] = {0, 0, 0};
// Inverse of gainMatrix
double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
gainMatrixInverse[0][0] = 1 / gainMatrix[0][0];
gainMatrixInverse[1][1] = 1 / gainMatrix[1][1];
gainMatrixInverse[2][2] = 1 / gainMatrix[2][2];
for (int i = 0; i < 3; i++) {
if (abs(qError[i]) < qErrorMin) {
qErrorLaw[i] = qErrorMin;
}
else {
qErrorLaw[i] = abs(qError[i]);
}
}
double qErrorLawNorm = VectorOperations<double>::norm(qErrorLaw, 3);
double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3,
3, 3);
MatrixOperations<double>::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3);
double gain1 = cInt * omMax / qErrorLawNorm;
double gainVector[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(qErrorLaw, gain1, gainVector, 3);
//------------------------------------------------------------------------------------------------
// Torque Calculations for the reaction wheels
//------------------------------------------------------------------------------------------------
double gainMatrixDiagonal[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double gainMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
gainMatrixDiagonal[0][0] = gainVector[0];
gainMatrixDiagonal[1][1] = gainVector[1];
gainMatrixDiagonal[2][2] = gainVector[2];
MatrixOperations<double>::multiply( *gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix), *gainMatrix, 3, 3, 3);
double pError[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*pMatrix, qError, pError, 3, 3, 1);
double pErrorSign[3] = {0, 0, 0};
// Inverse of gainMatrix
double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
gainMatrixInverse[0][0] = 1 / gainMatrix[0][0];
gainMatrixInverse[1][1] = 1 / gainMatrix[1][1];
gainMatrixInverse[2][2] = 1 / gainMatrix[2][2];
for (int i = 0; i < 3; i++) {
if (abs(pError[i]) > 1) {
pErrorSign[i] = sign(pError[i]);
} else {
pErrorSign[i] = pError[i];
}
}
// Torque for quaternion error
double torqueQuat[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1);
VectorOperations<double>::mulScalar(torqueQuat, -1, torqueQuat, 3);
double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3);
//------------------------------------------------------------------------------------------------
// Torque Calculations for the reaction wheels
//------------------------------------------------------------------------------------------------
double pError[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*pMatrix, qError, pError, 3, 3, 1);
double pErrorSign[3] = {0, 0, 0};
for (int i = 0; i < 3; i++) {
if (abs(pError[i]) > 1) {
pErrorSign[i] = sign(pError[i]);
}
else {
pErrorSign[i] = pError[i];
}
}
// Torque for quaternion error
double torqueQuat[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1);
VectorOperations<double>::mulScalar(torqueQuat, -1, torqueQuat, 3);
// Torque for rate error
double torqueRate[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1);
VectorOperations<double>::mulScalar(torqueRate, cInt, torqueRate, 3);
VectorOperations<double>::mulScalar(torqueRate, -1, torqueRate, 3);
// Final commanded Torque for every reaction wheel
double torque[3] = {0, 0, 0};
VectorOperations<double>::add(torqueRate, torqueQuat, torque, 3);
MatrixOperations<double>::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1);
// Torque for rate error
double torqueRate[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1);
VectorOperations<double>::mulScalar(torqueRate, cInt, torqueRate, 3);
VectorOperations<double>::mulScalar(torqueRate, -1, torqueRate, 3);
// Final commanded Torque for every reaction wheel
double torque[3] = {0, 0, 0};
VectorOperations<double>::add(torqueRate, torqueQuat, torque, 3);
MatrixOperations<double>::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1);
}
void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,

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

@ -8,95 +8,91 @@
#ifndef CHOLESKYDECOMPOSITION_H_
#define CHOLESKYDECOMPOSITION_H_
#include <math.h>
//typedef unsigned int uint8_t;
// typedef unsigned int uint8_t;
template<typename T1, typename T2=T1, typename T3=T2>
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;
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);
}
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];
}
}
}
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;
return 0; /* success */
}
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];
}
}
}
// 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 */
}
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];
}
}
// 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 */
}
return 0; /* success */
}
};
#endif /* CONTRIB_MATH_CHOLESKYDECOMPOSITION_H_ */