eive/eive-obsw
eive/eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests 3 Releases 117 Wiki Activity

MEKF cleanup

Browse Source
This commit is contained in:
Marius Eggert 2022-12-12 14:45:20 +01:00
parent 5fe3ac09a5
commit 46945a8674
1 changed files with 4 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
mission/controller/acs/MultiplicativeKalmanFilter.cpp
View File

@ -1,10 +1,3 @@
/*
* MultiplicativeKalmanFilter.cpp
*
* Created on: 4 Feb 2022
* Author: rooob
*/
#include "MultiplicativeKalmanFilter.h"
#include <fsfw/datapool/PoolReadGuard.h>
@ -31,7 +24,7 @@ MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {}
void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_) {
inertiaEIVE = &(acsParameters_->inertiaEIVE);
kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); /*Sensor noises also here*/
kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters);
}
void MultiplicativeKalmanFilter::reset() {}
@ -43,14 +36,11 @@ void MultiplicativeKalmanFilter::init(
// check for valid mag/sun
if (validMagField_ && validSS && validSSModel && validMagModel) {
validInit = true;
// AcsParameters mekfEstParams;
// loadAcsParameters(&mekfEstParams);
// QUEST ALGO -----------------------------------------------------------------------
double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0;
sigmaSun = kalmanFilterParameters->sensorNoiseSS;
sigmaMag = kalmanFilterParameters->sensorNoiseMAG;
sigmaGyro = 0.1 * 3.141 / 180; // acs parameters
sigmaGyro = kalmanFilterParameters->sensorNoiseGYR;
double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
normSunJ[3] = {0, 0, 0};
@ -138,7 +128,6 @@ void MultiplicativeKalmanFilter::init(
matrixMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
matrixSunMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
matrixMagSun[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
/* vector*transpose(vector)*/
MatrixOperations<double>::multiply(sunEstB, sunEstB, *matrixSun, 3, 1, 3);
MatrixOperations<double>::multiply(magEstB, magEstB, *matrixMag, 3, 1, 3);
MatrixOperations<double>::multiply(sunEstB, magEstB, *matrixSunMag, 3, 1, 3);
@ -201,8 +190,6 @@ void MultiplicativeKalmanFilter::init(
initialCovarianceMatrix[5][3] = initGyroCov[2][0];
initialCovarianceMatrix[5][4] = initGyroCov[2][1];
initialCovarianceMatrix[5][5] = initGyroCov[2][2];
// initialCovarianceMatrix[][] = {{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},
//{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}};
} else {
// no initialisation possible, no valid measurements
validInit = false;
@ -216,8 +203,6 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel,
const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData) {
// Check for GYR Measurements
// AcsParameters mekfEstParams;
// loadAcsParameters(&mekfEstParams);
int MDF = 0; // Matrix Dimension Factor
if (!validGYRs_) {
{
@ -934,10 +919,6 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
double errStateQuat[3] = {errStateVec[0], errStateVec[1], errStateVec[2]};
double errStateGyroBias[3] = {errStateVec[3], errStateVec[4], errStateVec[5]};
sif::debug << "errQ=[" << errStateQuat[0] << "," << errStateQuat[1] << "," << errStateQuat[2]
<< "]" << std::endl;
sif::debug << "errW_bias=[" << errStateGyroBias[0] << "," << errStateGyroBias[1] << ","
<< errStateGyroBias[2] << "]" << std::endl;
// State Vector Elements
double xi1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
@ -966,25 +947,18 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
biasGYR[2] = updatedGyroBias[2];
/* ----------- PROPAGATION ----------*/
// double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR;
// double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR;
double sigmaU = 3 * 3.141 / 180 / 3600;
double sigmaV = 3 * 0.0043 * 3.141 / sqrt(10) / 180;
double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR;
double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR;
double discTimeMatrix[6][6] = {{-1, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0},
{0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
double rotRateEst[3] = {0, 0, 0};
sif::debug << "MEKF rateGYR= " << rateGYRs_[0] << " " << rateGYRs_[1] << " " << rateGYRs_[2]
<< std::endl;
sif::debug << "MEKF bias= " << updatedGyroBias[0] << " " << updatedGyroBias[1] << " "
<< updatedGyroBias[2] << std::endl;
VectorOperations<double>::subtract(rateGYRs_, updatedGyroBias, rotRateEst, 3);
double normRotEst = VectorOperations<double>::norm(rotRateEst, 3);
double crossRotEst[3][3] = {{0, -rotRateEst[2], rotRateEst[1]},
{rotRateEst[2], 0, -rotRateEst[0]},
{-rotRateEst[1], rotRateEst[0], 0}};
// ToDo
// Discrete Process Noise Covariance Q
double discProcessNoiseCov[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
@ -1072,7 +1046,6 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
discProcessNoiseCov[5][4] = covQ22[2][1];
discProcessNoiseCov[5][5] = covQ22[2][2];
// ToDo
// State Transition Matrix phi
double phi11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
phi12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},