diff --git a/dummies/GyroL3GD20Dummy.cpp b/dummies/GyroL3GD20Dummy.cpp index a800cd62..20309c63 100644 --- a/dummies/GyroL3GD20Dummy.cpp +++ b/dummies/GyroL3GD20Dummy.cpp @@ -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({1.2}, true)); diff --git a/dummies/ImtqDummy.cpp b/dummies/ImtqDummy.cpp index 7f64d977..fdc7b009 100644 --- a/dummies/ImtqDummy.cpp +++ b/dummies/ImtqDummy.cpp @@ -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({0})); - localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, new PoolEntry({0.0,0.0,0.0})); + localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, new PoolEntry({0.0, 0.0, 0.0})); localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::MTM_RAW, new PoolEntry({0.12, 0.76, -0.45}, true)); localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry({0})); diff --git a/dummies/SusDummy.cpp b/dummies/SusDummy.cpp index 28f85969..c0aed6dd 100644 --- a/dummies/SusDummy.cpp +++ b/dummies/SusDummy.cpp @@ -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({0}, 1, true)); - localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC, - new PoolEntry({0, 0, 0, 0, 0, 0},true)); + localDataPoolMap.emplace(SUS::SusPoolIds::TEMPERATURE_C, new PoolEntry({0}, 1, true)); + localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC, + new PoolEntry({0, 0, 0, 0, 0, 0}, true)); - return returnvalue::OK; + return returnvalue::OK; } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index ed896d8e..1c4996b3 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -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 mgm0VecRaw = PoolEntry(3); diff --git a/mission/controller/CMakeLists.txt b/mission/controller/CMakeLists.txt index 463e6a5d..6177a14b 100644 --- a/mission/controller/CMakeLists.txt +++ b/mission/controller/CMakeLists.txt @@ -3,4 +3,4 @@ if(TGT_BSP MATCHES "arm/q7s" OR TGT_BSP MATCHES "") AcsController.cpp) endif() -add_subdirectory(acs) \ No newline at end of file +add_subdirectory(acs) diff --git a/mission/controller/acs/CMakeLists.txt b/mission/controller/acs/CMakeLists.txt index a379197d..3c4a3475 100644 --- a/mission/controller/acs/CMakeLists.txt +++ b/mission/controller/acs/CMakeLists.txt @@ -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) diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp index 1dc3a2e2..7020742a 100644 --- a/mission/controller/acs/SusConverter.cpp +++ b/mission/controller/acs/SusConverter.cpp @@ -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; } diff --git a/mission/controller/acs/SusConverter.h b/mission/controller/acs/SusConverter.h index 10079e90..b3829827 100644 --- a/mission/controller/acs/SusConverter.h +++ b/mission/controller/acs/SusConverter.h @@ -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, diff --git a/mission/controller/acs/config/classIds.h b/mission/controller/acs/config/classIds.h index e714ff33..ccf6b616 100644 --- a/mission/controller/acs/config/classIds.h +++ b/mission/controller/acs/config/classIds.h @@ -5,15 +5,14 @@ #include 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_ */ diff --git a/mission/controller/acs/control/CMakeLists.txt b/mission/controller/acs/control/CMakeLists.txt index dc540435..2d40ecc5 100644 --- a/mission/controller/acs/control/CMakeLists.txt +++ b/mission/controller/acs/control/CMakeLists.txt @@ -1,5 +1,2 @@ -target_sources( - ${LIB_EIVE_MISSION} - PRIVATE Detumble.cpp - PtgCtrl.cpp - SafeCtrl.cpp) \ No newline at end of file +target_sources(${LIB_EIVE_MISSION} PRIVATE Detumble.cpp PtgCtrl.cpp + SafeCtrl.cpp) diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 7421226d..4c8217dc 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -6,30 +6,24 @@ * Author: Robin Marquardt */ - #include "Detumble.h" -#include "../util/MathOperations.h" -#include + #include #include #include #include #include +#include +#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, diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index 375f67aa..bea9b1bf 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -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 -#include -#include #include +#include +#include +#include +#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_*/ - diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index e06b576f..07b595ec 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -5,10 +5,8 @@ * Author: Robin Marquardt */ - - #include "PtgCtrl.h" -#include "../util/MathOperations.h" + #include #include #include @@ -16,100 +14,96 @@ #include #include -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::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::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::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::norm(qErrorLaw, 3); + double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3, + 3, 3); + MatrixOperations::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3); - double gain1 = cInt * omMax / qErrorLawNorm; - double gainVector[3] = {0, 0, 0}; - VectorOperations::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::multiply( *gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix), *gainMatrix, 3, 3, 3); + double pError[3] = {0, 0, 0}; + MatrixOperations::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::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1); + VectorOperations::mulScalar(torqueQuat, -1, torqueQuat, 3); - double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MatrixOperations::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3, 3, 3); - MatrixOperations::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3); - - //------------------------------------------------------------------------------------------------ - // Torque Calculations for the reaction wheels - //------------------------------------------------------------------------------------------------ - - double pError[3] = {0, 0, 0}; - MatrixOperations::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::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1); - VectorOperations::mulScalar(torqueQuat, -1, torqueQuat, 3); - -// Torque for rate error - double torqueRate[3] = {0, 0, 0}; - MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1); - VectorOperations::mulScalar(torqueRate, cInt, torqueRate, 3); - VectorOperations::mulScalar(torqueRate, -1, torqueRate, 3); - -// Final commanded Torque for every reaction wheel - double torque[3] = {0, 0, 0}; - VectorOperations::add(torqueRate, torqueQuat, torque, 3); - MatrixOperations::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1); + // Torque for rate error + double torqueRate[3] = {0, 0, 0}; + MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1); + VectorOperations::mulScalar(torqueRate, cInt, torqueRate, 3); + VectorOperations::mulScalar(torqueRate, -1, torqueRate, 3); + // Final commanded Torque for every reaction wheel + double torque[3] = {0, 0, 0}; + VectorOperations::add(torqueRate, torqueQuat, torque, 3); + MatrixOperations::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1); } void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate, diff --git a/mission/controller/acs/util/CholeskyDecomposition.h b/mission/controller/acs/util/CholeskyDecomposition.h index 38ce0bcc..667f9f63 100644 --- a/mission/controller/acs/util/CholeskyDecomposition.h +++ b/mission/controller/acs/util/CholeskyDecomposition.h @@ -8,95 +8,91 @@ #ifndef CHOLESKYDECOMPOSITION_H_ #define CHOLESKYDECOMPOSITION_H_ #include -//typedef unsigned int uint8_t; +// typedef unsigned int uint8_t; -template +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; + 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_ */