afmt

2022-12-01 15:56:55 +01:00 · 2022-12-01 15:56:55 +01:00 · 828738ba0e
commit 828738ba0e
parent a3a919437e
14 changed files with 217 additions and 240 deletions
--- 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<float>({1.2}, true));
--- 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<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}));
--- 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<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;
 }
--- 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<float> mgm0VecRaw = PoolEntry<float>(3);
--- 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)
--- 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;
 }
--- 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,
--- a/mission/controller/acs/config/classIds.h
+++ b/mission/controller/acs/config/classIds.h
@ -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_ */
--- 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)
+target_sources(${LIB_EIVE_MISSION} PRIVATE Detumble.cpp PtgCtrl.cpp
+                                           SafeCtrl.cpp)
--- 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 <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,
--- 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 <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_*/
-
--- 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 <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,
--- 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 <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_ */