2022-09-20 14:15:55 +02:00
|
|
|
/*
|
|
|
|
|
* Navigation.cpp
|
|
|
|
|
*
|
|
|
|
|
* Created on: 23 May 2022
|
|
|
|
|
* Author: Robin Marquardt
|
|
|
|
|
*/
|
|
|
|
|
|
|
|
|
|
#include "Navigation.h"
|
2022-09-27 11:06:11 +02:00
|
|
|
#include "util/MathOperations.h"
|
|
|
|
|
#include "util/CholeskyDecomposition.h"
|
2022-09-20 14:15:55 +02:00
|
|
|
#include <math.h>
|
2022-09-27 11:06:11 +02:00
|
|
|
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
|
|
|
|
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
|
|
|
|
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
|
|
|
|
|
2022-09-20 14:15:55 +02:00
|
|
|
|
|
|
|
|
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 ?
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
