added MKF

mission/controller/acs/MultiplicativeKalmanFilter.h

@@ -0,0 +1,107 @@
+/*
+ * MultiplicativeKalmanFilter.h
+ *
+ *  Created on: 4 Feb 2022
+ *      Author: Robin Marquardt
+ *
+ *      @brief: This class handles the calculation of an estimated quaternion and the gyro bias by
+ *      means of the spacecraft attitude sensors
+ *
+ *      @note: A description of the used algorithms can be found in the bachelor thesis of Robin Marquardt
+ *      https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=500811
+ */
+
+#ifndef MULTIPLICATIVEKALMANFILTER_H_
+#define MULTIPLICATIVEKALMANFILTER_H_
+
+#include <stdint.h> //uint8_t
+#include <time.h> /*purpose, timeval ?*/
+#include "acs/config/classIds.h"
+//#include <_timeval.h>
+
+#include "AcsParameters.h"
+
+class MultiplicativeKalmanFilter : public HasReturnvaluesIF {
+public:
+	/* @brief: Constructor
+	 * @param: acsParameters_   Pointer to object which defines the ACS configuration parameters
+	 */
+	MultiplicativeKalmanFilter(AcsParameters *acsParameters_);
+	virtual ~MultiplicativeKalmanFilter();
+
+	void reset(); // NOT YET DEFINED - should only reset all mekf variables
+
+	/* @brief: init() - This function initializes the Kalman Filter and will provide the first quaternion through
+	 * the QUEST algorithm
+	 * @param:  magneticField_  magnetic field vector in the body frame
+	 * 			sunDir_ 		sun direction vector in the body frame
+	 * 			sunDirJ 		sun direction vector in the ECI frame
+	 * 			magFieldJ 		magnetic field vector in the ECI frame
+	 */
+	void init(const double *magneticField_, const bool *validMagField_,
+			const double *sunDir_, const bool *validSS,
+			const double *sunDirJ, const bool *validSSModel,
+			const double *magFieldJ,const bool *validMagModel);
+
+	/* @brief:  mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter for the current step
+	 * 			after the initalization
+	 * @param:  quaternionSTR   Star Tracker Quaternion between from body to ECI frame
+	 * 			rateRMUs_ 		Estimated satellite rotation rate from the Rate Measurement Units [rad/s]
+	 * 			magneticField_  magnetic field vector in the body frame
+	 * 			sunDir_ 		sun direction vector in the body frame
+	 * 			sunDirJ 		sun direction vector in the ECI frame
+	 * 			magFieldJ 		magnetic field vector in the ECI frame
+	 * 			outputQuat 		Stores the calculated quaternion
+	 * 			outputSatRate 	Stores the adjusted satellite rate
+	 * @return 	ReturnValue_t	Feedback of this class, KALMAN_NO_RMU_MEAS if no satellite rate from the sensors was provided,
+	 * 							KALMAN_NO_MODEL if no sunDirJ or magFieldJ was given from the model calculations,
+	 * 							KALMAN_INVERSION_FAILED if the calculation of the Gain matrix was not possible,
+	 * 							RETURN_OK else
+	 */
+	ReturnValue_t mekfEst(
+			const double *quaternionSTR, const bool *validSTR_,
+			const double *rateRMUs_, const bool *validRMUs_,
+			const double *magneticField_, const bool *validMagField_,
+			const double *sunDir_, const bool *validSS,
+			const double *sunDirJ, const bool *validSSModel,
+			const double *magFieldJ,const bool *validMagModel,
+			double *outputQuat, double *outputSatRate);
+
+
+	// Declaration of Events (like init) and memberships
+	//static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND (/config/returnvalues/classIDs.h)
+	//static const Event RESET  = MAKE_EVENT(1,severity::INFO);//typedef uint32_t Event (Event.h), should be
+	// resetting Mekf
+	static const uint8_t INTERFACE_ID = CLASS_ID::KALMAN;
+	static const ReturnValue_t KALMAN_NO_RMU_MEAS = MAKE_RETURN_CODE(0x01);
+	static const ReturnValue_t KALMAN_NO_MODEL = MAKE_RETURN_CODE(0x02);
+	static const ReturnValue_t KALMAN_INVERSION_FAILED = MAKE_RETURN_CODE(0x03);
+
+private:
+/*Parameters*/
+	AcsParameters::InertiaEIVE* inertiaEIVE;
+	AcsParameters::KalmanFilterParameters* kalmanFilterParameters;
+	double quaternion_STR_SB[4];
+	bool validInit;
+	double sampleTime = 0.1;
+
+
+/*States*/
+	double initialQuaternion[4]; /*after reset?QUEST*/
+	double initialCovarianceMatrix[6][6];/*after reset?QUEST*/
+	double propagatedQuaternion[4]; /*Filter Quaternion for next step*/
+	bool validMekf;
+	uint8_t sensorsAvail;
+
+/*Outputs*/
+	double quatBJ[4]; /* Output Quaternion */
+	double biasRMU[3]; /*Between measured and estimated sat Rate*/
+/*Parameter INIT*/
+	//double alpha, gamma, beta;
+/*Functions*/
+	void loadAcsParameters(AcsParameters *acsParameters_);
+};
+
+
+
+#endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */