added Navitation

mission/controller/acs/Navigation.h

@@ -0,0 +1,35 @@
+/*
+ * Navigation.h
+ *
+ *  Created on: 19 Apr 2022
+ *      Author: Robin Marquardt
+ */
+
+#ifndef NAVIGATION_H_
+#define NAVIGATION_H_
+
+
+#include "AcsParameters.h"
+#include "SensorProcessing.h"
+#include <acs/MultiplicativeKalmanFilter.h>
+#include <acs/SensorValues.h>
+#include <acs/OutputValues.h>
+
+class Navigation{
+public:
+	Navigation(AcsParameters *acsParameters_); //Input mode ?
+	virtual ~Navigation();
+
+	void useMekf(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, ReturnValue_t *mekfValid);
+	void processSensorData();
+
+protected:
+
+private:
+	MultiplicativeKalmanFilter multiplicativeKalmanFilter;
+	AcsParameters acsParameters;
+	bool kalmanInit = false;
+};
+
+#endif /* ACS_NAVIGATION_H_ */
+

mission/controller/acs/Navitation.cpp

@@ -0,0 +1,49 @@
+/*
+ * Navigation.cpp
+ *
+ *  Created on: 23 May 2022
+ *      Author: Robin Marquardt
+ */
+
+#include "Navigation.h"
+#include <math.h>
+#include <acs/math/MathOperations.h>
+#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
+#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
+#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
+#include <acs/math/CholeskyDecomposition.h>
+
+Navigation::Navigation(AcsParameters *acsParameters_): multiplicativeKalmanFilter(acsParameters_){
+	acsParameters = *acsParameters_;
+}
+
+Navigation::~Navigation(){
+
+}
+
+void Navigation::useMekf(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, ReturnValue_t *mekfValid){
+
+	if (kalmanInit) {
+		*mekfValid = multiplicativeKalmanFilter.mekfEst(sensorValues->quatJB, &sensorValues->quatJBValid,
+				outputValues->satRateEst, &outputValues->satRateEstValid,
+				outputValues->magFieldEst, &outputValues->magFieldEstValid,
+				outputValues->sunDirEst, &outputValues->sunDirEstValid,
+				outputValues->sunDirModel, &outputValues->sunDirModelValid,
+				outputValues->magFieldModel, &outputValues->magFieldModelValid,
+				outputValues->quatMekfBJ, outputValues->satRateMekf); // VALIDS FOR QUAT AND RATE ??
+
+	}
+	else {
+		multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid,
+				outputValues->sunDirEst, &outputValues->sunDirEstValid,
+				outputValues->sunDirModel, &outputValues->sunDirModelValid,
+				outputValues->magFieldModel, &outputValues->magFieldModelValid);
+		kalmanInit = true;
+		*mekfValid = 0;
+
+//		Maybe we need feedback from kalmanfilter to identify if there was a problem with the init of kalman filter
+//		where does this class know from that kalman filter was not initialized ?
+	}
+}
+
+