eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

fixed int32_t to double warnings. reformats

Browse Source
This commit is contained in:
Marius Eggert 2022-10-24 10:29:57 +02:00
parent 0d3509b991
commit 8b23fd3dd2
4 changed files with 1186 additions and 1248 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -6,115 +6,97 @@
*/ */
#include "MultiplicativeKalmanFilter.h" #include "MultiplicativeKalmanFilter.h"
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
#include <string.h>
#include <stdio.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h> #include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h> #include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h> #include <fsfw/globalfunctions/math/VectorOperations.h>
#include <stdio.h>
#include <string.h>
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
/*Initialisation of values for parameters in constructor*/ /*Initialisation of values for parameters in constructor*/
MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_) : MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_)
initialQuaternion { 0.5, 0.5, 0.5, 0.5 },initialCovarianceMatrix : initialQuaternion{0.5, 0.5, 0.5, 0.5},
{{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}, 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}}{ {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}} {
loadAcsParameters(acsParameters_); loadAcsParameters(acsParameters_);
} }
MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() { MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {}
} void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_) {
void MultiplicativeKalmanFilter::loadAcsParameters(
AcsParameters *acsParameters_) {
inertiaEIVE = &(acsParameters_->inertiaEIVE); inertiaEIVE = &(acsParameters_->inertiaEIVE);
kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); /*Sensor noises also here*/ kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); /*Sensor noises also here*/
} }
void MultiplicativeKalmanFilter::reset() { void MultiplicativeKalmanFilter::reset() {}
} void MultiplicativeKalmanFilter::init(
const double *magneticField_, const bool *validMagField_, const double *sunDir_,
void MultiplicativeKalmanFilter::init(const double *magneticField_, const bool *validMagField_, const bool *validSS, const double *sunDirJ, const bool *validSSModel, const double *magFieldJ,
const double *sunDir_, const bool *validSS, const bool *validMagModel) { // valids for "model measurements"?
const double *sunDirJ, const bool *validSSModel,
const double *magFieldJ,const bool *validMagModel) { // valids for "model measurements"?
// check for valid mag/sun // check for valid mag/sun
if (*validMagField_ && *validSS && *validSSModel && *validMagModel) { if (*validMagField_ && *validSS && *validSSModel && *validMagModel) {
validInit = true; validInit = true;
//AcsParameters mekfEstParams; // AcsParameters mekfEstParams;
//loadAcsParameters(&mekfEstParams); // loadAcsParameters(&mekfEstParams);
// QUEST ALGO ----------------------------------------------------------------------- // QUEST ALGO -----------------------------------------------------------------------
double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0; double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0;
sigmaSun = kalmanFilterParameters->sensorNoiseSS; sigmaSun = kalmanFilterParameters->sensorNoiseSS;
sigmaMag = kalmanFilterParameters->sensorNoiseMAG; sigmaMag = kalmanFilterParameters->sensorNoiseMAG;
sigmaGyro = 0.1*3.141/180; // acs parameters sigmaGyro = 0.1 * 3.141 / 180; // acs parameters
double normMagB[3] = {0,0,0}, normSunB[3] = {0,0,0}, double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
normMagJ[3] = {0,0,0}, normSunJ[3] = {0,0,0}; normSunJ[3] = {0, 0, 0};
VectorOperations<double>::normalize(magneticField_, normMagB, 3); // b2 VectorOperations<double>::normalize(magneticField_, normMagB, 3); // b2
VectorOperations<double>::normalize(sunDir_, normSunB, 3); //b1 VectorOperations<double>::normalize(sunDir_, normSunB, 3); // b1
VectorOperations<double>::normalize(magFieldJ, normMagJ, 3); //r2 VectorOperations<double>::normalize(magFieldJ, normMagJ, 3); // r2
VectorOperations<double>::normalize(sunDirJ, normSunJ, 3); //r1 VectorOperations<double>::normalize(sunDirJ, normSunJ, 3); // r1
double thirdAxis_B[3] = {0,0,0}, thirdAxis_J[3] = {0,0,0}; double thirdAxis_B[3] = {0, 0, 0}, thirdAxis_J[3] = {0, 0, 0};
VectorOperations<double>::cross(normSunJ, normMagJ, thirdAxis_J); VectorOperations<double>::cross(normSunJ, normMagJ, thirdAxis_J);
VectorOperations<double>::cross(normSunB, normMagB, thirdAxis_B); VectorOperations<double>::cross(normSunB, normMagB, thirdAxis_B);
VectorOperations<double>::normalize(thirdAxis_J, thirdAxis_J, 3); //rx VectorOperations<double>::normalize(thirdAxis_J, thirdAxis_J, 3); // rx
VectorOperations<double>::normalize(thirdAxis_B, thirdAxis_B, 3); //bx VectorOperations<double>::normalize(thirdAxis_B, thirdAxis_B, 3); // bx
double weigthSun= 1 / (sigmaSun * sigmaSun); //a1 double weigthSun = 1 / (sigmaSun * sigmaSun); // a1
double weigthMag= 1 / (sigmaMag * sigmaMag); //a2 double weigthMag = 1 / (sigmaMag * sigmaMag); // a2
double a[3] = {0,0,0}, b[3] = {0,0,0}; double a[3] = {0, 0, 0}, b[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(normSunB, weigthSun, a, 3); //a VectorOperations<double>::mulScalar(normSunB, weigthSun, a, 3); // a
VectorOperations<double>::mulScalar(normMagB, weigthMag, b, 3); //b VectorOperations<double>::mulScalar(normMagB, weigthMag, b, 3); // b
double thirdAxisCross[3] = {0,0,0}, weightACross[3] = {0,0,0}, double thirdAxisCross[3] = {0, 0, 0}, weightACross[3] = {0, 0, 0}, weightBCross[3] = {0, 0, 0},
weightBCross[3] = {0,0,0},weigthCrossSum[3] = {0,0,0}; weigthCrossSum[3] = {0, 0, 0};
VectorOperations<double>::cross(thirdAxis_B, thirdAxis_J, VectorOperations<double>::cross(thirdAxis_B, thirdAxis_J,
thirdAxisCross); //cross(bx,rx) thirdAxisCross); // cross(bx,rx)
VectorOperations<double>::cross(a, normSunJ, weightACross); VectorOperations<double>::cross(a, normSunJ, weightACross);
VectorOperations<double>::cross(b, normMagJ, weightBCross); VectorOperations<double>::cross(b, normMagJ, weightBCross);
VectorOperations<double>::add(weightACross, weightBCross, VectorOperations<double>::add(weightACross, weightBCross, weigthCrossSum);
weigthCrossSum);
double thirdAxisSum[3] = {0,0,0}, sum2[3] = {0,0,0}; double thirdAxisSum[3] = {0, 0, 0}, sum2[3] = {0, 0, 0};
VectorOperations<double>::add(thirdAxis_B, thirdAxis_J, thirdAxisSum); VectorOperations<double>::add(thirdAxis_B, thirdAxis_J, thirdAxisSum);
VectorOperations<double>::add(weightACross, weightBCross, sum2); VectorOperations<double>::add(weightACross, weightBCross, sum2);
double alpha = 0, beta = 0, gamma = 0, constPlus = 0, constMin = 0; double alpha = 0, beta = 0, gamma = 0, constPlus = 0, constMin = 0;
alpha = (1 + VectorOperations<double>::dot(thirdAxis_B, thirdAxis_J)) alpha = (1 + VectorOperations<double>::dot(thirdAxis_B, thirdAxis_J)) *
* (VectorOperations<double>::dot(a, normSunJ) (VectorOperations<double>::dot(a, normSunJ) +
+ VectorOperations<double>::dot(b, normMagJ)) VectorOperations<double>::dot(b, normMagJ)) +
+ VectorOperations<double>::dot(thirdAxisCross, weigthCrossSum); VectorOperations<double>::dot(thirdAxisCross, weigthCrossSum);
beta = VectorOperations<double>::dot(thirdAxisSum, sum2); beta = VectorOperations<double>::dot(thirdAxisSum, sum2);
gamma = sqrt(alpha * alpha + beta * beta); gamma = sqrt(alpha * alpha + beta * beta);
constPlus = 1 constPlus = 1 / (2 * sqrt(gamma * (gamma + alpha) *
/ (2 (1 + VectorOperations<double>::dot(thirdAxis_B, thirdAxis_J))));
* sqrt( constMin = 1 / (2 * sqrt(gamma * (gamma - alpha) *
gamma * (gamma + alpha) (1 + VectorOperations<double>::dot(thirdAxis_B, thirdAxis_J))));
* (1
+ VectorOperations<double>::dot(
thirdAxis_B,
thirdAxis_J))));
constMin = 1
/ (2
* sqrt(
gamma * (gamma - alpha)
* (1
+ VectorOperations<double>::dot(
thirdAxis_B,
thirdAxis_J))));
/*Quaternion Computation*/ /*Quaternion Computation*/
double quat[3] = {0,0,0}, var1[3] = {0,0,0}, var2[3] = {0,0,0}; double quat[3] = {0, 0, 0}, var1[3] = {0, 0, 0}, var2[3] = {0, 0, 0};
if (alpha >= 0) { if (alpha >= 0) {
VectorOperations<double>::mulScalar(thirdAxisCross, gamma + alpha, VectorOperations<double>::mulScalar(thirdAxisCross, gamma + alpha, var1, 3);
var1, 3);
VectorOperations<double>::add(thirdAxis_B, thirdAxis_J, var2, 3); VectorOperations<double>::add(thirdAxis_B, thirdAxis_J, var2, 3);
VectorOperations<double>::mulScalar(var2, beta, var2, 3); VectorOperations<double>::mulScalar(var2, beta, var2, 3);
VectorOperations<double>::add(var1, var2, quat); VectorOperations<double>::add(var1, var2, quat);
@ -122,12 +104,9 @@ void MultiplicativeKalmanFilter::init(const double *magneticField_, const bool *
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
initialQuaternion[i] = quat[i]; initialQuaternion[i] = quat[i];
} }
initialQuaternion[3] = (gamma + alpha) initialQuaternion[3] =
* (1 (gamma + alpha) * (1 + VectorOperations<double>::dot(thirdAxis_B, thirdAxis_J));
+ VectorOperations<double>::dot(thirdAxis_B, VectorOperations<double>::mulScalar(initialQuaternion, constPlus, initialQuaternion, 4);
thirdAxis_J));
VectorOperations<double>::mulScalar(initialQuaternion,
constPlus, initialQuaternion, 4);
} else { } else {
double diffGammaAlpha = gamma - alpha; double diffGammaAlpha = gamma - alpha;
VectorOperations<double>::mulScalar(thirdAxisCross, beta, var1, 3); VectorOperations<double>::mulScalar(thirdAxisCross, beta, var1, 3);
@ -137,13 +116,8 @@ void MultiplicativeKalmanFilter::init(const double *magneticField_, const bool *
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
initialQuaternion[i] = quat[i]; initialQuaternion[i] = quat[i];
} }
initialQuaternion[3] = beta initialQuaternion[3] = beta * (1 + VectorOperations<double>::dot(thirdAxis_B, thirdAxis_J));
* (1 VectorOperations<double>::mulScalar(initialQuaternion, constMin, initialQuaternion, 4);
+ VectorOperations<double>::dot(thirdAxis_B,
thirdAxis_J));
VectorOperations<double>::mulScalar(initialQuaternion,
constMin, initialQuaternion, 4);
} }
propagatedQuaternion[0] = initialQuaternion[0]; propagatedQuaternion[0] = initialQuaternion[0];
propagatedQuaternion[1] = initialQuaternion[1]; propagatedQuaternion[1] = initialQuaternion[1];
@ -151,55 +125,43 @@ void MultiplicativeKalmanFilter::init(const double *magneticField_, const bool *
propagatedQuaternion[3] = initialQuaternion[3]; propagatedQuaternion[3] = initialQuaternion[3];
/*Initial Covariance Matrix------------------------------------ */ /*Initial Covariance Matrix------------------------------------ */
double dcmBJ[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, sunEstB[3] = {0, 0, 0},
sunEstB[3] = {0,0,0}, magEstB[3] = {0,0,0}; magEstB[3] = {0, 0, 0};
QuaternionOperations::toDcm(initialQuaternion, dcmBJ); QuaternionOperations::toDcm(initialQuaternion, dcmBJ);
MatrixOperations<double>::multiply(*dcmBJ, normSunJ, sunEstB, 3, 3, 1); MatrixOperations<double>::multiply(*dcmBJ, normSunJ, sunEstB, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmBJ, normMagJ, magEstB, 3, 3, MatrixOperations<double>::multiply(*dcmBJ, normMagJ, magEstB, 3, 3, 1);
1);
double matrixSun[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, double matrixSun[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
matrixMag[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, matrixMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
matrixSunMag[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}}; matrixMagSun[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
/* vector*transpose(vector)*/ /* vector*transpose(vector)*/
MatrixOperations<double>::multiply(sunEstB, sunEstB, *matrixSun, 3, 1, 3); MatrixOperations<double>::multiply(sunEstB, sunEstB, *matrixSun, 3, 1, 3);
MatrixOperations<double>::multiply(magEstB, magEstB, *matrixMag, 3, 1, 3); MatrixOperations<double>::multiply(magEstB, magEstB, *matrixMag, 3, 1, 3);
MatrixOperations<double>::multiply(sunEstB, magEstB, *matrixSunMag, 3, 1, 3); MatrixOperations<double>::multiply(sunEstB, magEstB, *matrixSunMag, 3, 1, 3);
MatrixOperations<double>::multiply(magEstB, sunEstB, *matrixMagSun, 3, 1, 3); MatrixOperations<double>::multiply(magEstB, sunEstB, *matrixMagSun, 3, 1, 3);
double partA[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, partB = 0; double partA[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, partB = 0;
MatrixOperations<double>::multiplyScalar(*matrixSun, MatrixOperations<double>::multiplyScalar(*matrixSun, weigthSun * weigthSun, *matrixSun, 3, 3);
weigthSun * weigthSun, *matrixSun, 3, 3); MatrixOperations<double>::multiplyScalar(*matrixMag, weigthMag * weigthMag, *matrixMag, 3, 3);
MatrixOperations<double>::multiplyScalar(*matrixMag, MatrixOperations<double>::add(*matrixSunMag, *matrixMagSun, *partA, 3, 3);
weigthMag * weigthMag, *matrixMag, 3, 3); double factor = weigthMag * weigthSun * VectorOperations<double>::dot(sunEstB, magEstB);
MatrixOperations<double>::add(*matrixSunMag, *matrixMagSun, *partA, 3,
3);
double factor = weigthMag * weigthSun
* VectorOperations<double>::dot(sunEstB, magEstB);
MatrixOperations<double>::multiplyScalar(*partA, factor, *partA, 3, 3); MatrixOperations<double>::multiplyScalar(*partA, factor, *partA, 3, 3);
MatrixOperations<double>::add(*partA, *matrixSun, *partA, 3, 3); MatrixOperations<double>::add(*partA, *matrixSun, *partA, 3, 3);
MatrixOperations<double>::add(*partA, *matrixMag, *partA, 3, 3); MatrixOperations<double>::add(*partA, *matrixMag, *partA, 3, 3);
double crossSunMag[3] = {0,0,0}; double crossSunMag[3] = {0, 0, 0};
double identityMatrix3[3][3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } }; double identityMatrix3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
VectorOperations<double>::cross(sunEstB, magEstB, crossSunMag); VectorOperations<double>::cross(sunEstB, magEstB, crossSunMag);
partB = partB = 1 / (weigthMag * weigthSun * pow(VectorOperations<double>::norm(crossSunMag, 3), 2));
1
/ (weigthMag * weigthSun
* pow(
VectorOperations<double>::norm(
crossSunMag, 3), 2));
MatrixOperations<double>::multiplyScalar(*partA, partB, *partA, 3, 3); MatrixOperations<double>::multiplyScalar(*partA, partB, *partA, 3, 3);
MatrixOperations<double>::add(*matrixSunMag, *identityMatrix3, *partA, 3, MatrixOperations<double>::add(*matrixSunMag, *identityMatrix3, *partA, 3, 3);
3);
factor = 1 / (weigthMag + weigthMag); factor = 1 / (weigthMag + weigthMag);
double questCovMatrix[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; double questCovMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiplyScalar(*partA, factor, MatrixOperations<double>::multiplyScalar(*partA, factor, *questCovMatrix, 3, 3);
*questCovMatrix, 3, 3); double initGyroCov[3][3] = {
double initGyroCov[3][3] = {{pow(sigmaGyro,2),0,0},{0,pow(sigmaGyro,2),0}, {pow(sigmaGyro, 2), 0, 0}, {0, pow(sigmaGyro, 2), 0}, {0, 0, pow(sigmaGyro, 2)}};
{0,0,pow(sigmaGyro,2)}};
initialCovarianceMatrix[0][0] = questCovMatrix[0][0]; initialCovarianceMatrix[0][0] = questCovMatrix[0][0];
initialCovarianceMatrix[0][1] = questCovMatrix[0][1]; initialCovarianceMatrix[0][1] = questCovMatrix[0][1];
initialCovarianceMatrix[0][2] = questCovMatrix[0][2]; initialCovarianceMatrix[0][2] = questCovMatrix[0][2];
@ -236,7 +198,7 @@ void MultiplicativeKalmanFilter::init(const double *magneticField_, const bool *
initialCovarianceMatrix[5][3] = initGyroCov[2][0]; initialCovarianceMatrix[5][3] = initGyroCov[2][0];
initialCovarianceMatrix[5][4] = initGyroCov[2][1]; initialCovarianceMatrix[5][4] = initGyroCov[2][1];
initialCovarianceMatrix[5][5] = initGyroCov[2][2]; 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}, // 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}}; //{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}};
} else { } else {
// no initialisation possible, no valid measurements // no initialisation possible, no valid measurements
@ -246,58 +208,47 @@ void MultiplicativeKalmanFilter::init(const double *magneticField_, const bool *
// --------------- MEKF DISCRETE TIME STEP ------------------------------- // --------------- MEKF DISCRETE TIME STEP -------------------------------
ReturnValue_t MultiplicativeKalmanFilter::mekfEst( ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
const double *quaternionSTR, const bool *validSTR_, const double *quaternionSTR, const bool *validSTR_, const double *rateGYRs_,
const double *rateRMUs_, const bool *validRMUs_, const bool *validGYRs_, const double *magneticField_, const bool *validMagField_,
const double *magneticField_, const bool *validMagField_, const double *sunDir_, const bool *validSS, const double *sunDirJ, const bool *validSSModel,
const double *sunDir_, const bool *validSS, const double *magFieldJ, const bool *validMagModel, double *outputQuat, double *outputSatRate) {
const double *sunDirJ, const bool *validSSModel, // Check for GYR Measurements
const double *magFieldJ,const bool *validMagModel, // AcsParameters mekfEstParams;
double *outputQuat, double *outputSatRate) { // loadAcsParameters(&mekfEstParams);
// Check for RMU Measurements
//AcsParameters mekfEstParams;
//loadAcsParameters(&mekfEstParams);
int MDF = 0; // Matrix Dimension Factor int MDF = 0; // Matrix Dimension Factor
if (!(*validRMUs_)){ if (!(*validGYRs_)) {
validMekf = false; validMekf = false;
return KALMAN_NO_RMU_MEAS; return KALMAN_NO_GYR_MEAS;
} }
// Check for Model Calculations // Check for Model Calculations
else if (!(*validSSModel) || !(*validMagModel)){ else if (!(*validSSModel) || !(*validMagModel)) {
validMekf = false; validMekf = false;
return KALMAN_NO_MODEL; return KALMAN_NO_MODEL;
} }
// Check Measurements available from SS, MAG, STR // Check Measurements available from SS, MAG, STR
if (*validSS && *validMagField_ && *validSTR_){ if (*validSS && *validMagField_ && *validSTR_) {
sensorsAvail = 1; sensorsAvail = 1;
MDF = 9; MDF = 9;
} } else if (*validSS && *validMagField_ && !(*validSTR_)) {
else if (*validSS && *validMagField_ && !(*validSTR_)){
sensorsAvail = 2; sensorsAvail = 2;
MDF = 6; MDF = 6;
} } else if (*validSS && !(*validMagField_) && *validSTR_) {
else if (*validSS && !(*validMagField_) && *validSTR_){
sensorsAvail = 3; sensorsAvail = 3;
MDF = 6; MDF = 6;
} } else if (!(*validSS) && *validMagField_ && *validSTR_) {
else if (!(*validSS) && *validMagField_ && *validSTR_){
sensorsAvail = 4; sensorsAvail = 4;
MDF = 6; MDF = 6;
} } else if (*validSS && !(*validMagField_) && !(*validSTR_)) {
else if (*validSS && !(*validMagField_) && !(*validSTR_)){
sensorsAvail = 5; sensorsAvail = 5;
MDF = 3; MDF = 3;
} } else if (!(*validSS) && *validMagField_ && !(*validSTR_)) {
else if (!(*validSS) && *validMagField_ && !(*validSTR_)){
sensorsAvail = 6; sensorsAvail = 6;
MDF = 3; MDF = 3;
} } else if (!(*validSS) && !(*validMagField_) && *validSTR_) {
else if (!(*validSS) && !(*validMagField_) && *validSTR_){
sensorsAvail = 7; sensorsAvail = 7;
MDF = 3; MDF = 3;
} } else {
else{ sensorsAvail = 8; // no measurements
sensorsAvail = 8; //no measurements
validMekf = false; validMekf = false;
return returnvalue::FAILED; return returnvalue::FAILED;
} }
@ -308,47 +259,48 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
sigmaMag = kalmanFilterParameters->sensorNoiseMAG; sigmaMag = kalmanFilterParameters->sensorNoiseMAG;
sigmaStr = kalmanFilterParameters->sensorNoiseSTR; sigmaStr = kalmanFilterParameters->sensorNoiseSTR;
double normMagB[3] = {0,0,0}, normSunB[3] = {0,0,0}, double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
normMagJ[3] = {0,0,0}, normSunJ[3] = {0,0,0}; normSunJ[3] = {0, 0, 0};
VectorOperations<double>::normalize(magneticField_, normMagB, 3); // b2 VectorOperations<double>::normalize(magneticField_, normMagB, 3); // b2
VectorOperations<double>::normalize(sunDir_, normSunB, 3); //b1 VectorOperations<double>::normalize(sunDir_, normSunB, 3); // b1
VectorOperations<double>::normalize(magFieldJ, normMagJ, 3); //r2 VectorOperations<double>::normalize(magFieldJ, normMagJ, 3); // r2
VectorOperations<double>::normalize(sunDirJ, normSunJ, 3); //r1 VectorOperations<double>::normalize(sunDirJ, normSunJ, 3); // r1
/*-------GAIN - UPDATE - STEP------*/ /*-------GAIN - UPDATE - STEP------*/
double covMatrix11[3][3] = {{pow(sigmaSun,2),0,0},{0,pow(sigmaSun,2),0},{0,0,pow(sigmaSun,2)}}; double covMatrix11[3][3] = {
double covMatrix22[3][3] = {{pow(sigmaMag,2),0,0},{0,pow(sigmaMag,2),0},{0,0,pow(sigmaMag,2)}}; {pow(sigmaSun, 2), 0, 0}, {0, pow(sigmaSun, 2), 0}, {0, 0, pow(sigmaSun, 2)}};
double covMatrix33[3][3] = {{pow(sigmaStr,2),0,0},{0,pow(sigmaStr,2),0},{0,0,pow(sigmaStr,2)}}; double covMatrix22[3][3] = {
{pow(sigmaMag, 2), 0, 0}, {0, pow(sigmaMag, 2), 0}, {0, 0, pow(sigmaMag, 2)}};
double covMatrix33[3][3] = {
{pow(sigmaStr, 2), 0, 0}, {0, pow(sigmaStr, 2), 0}, {0, 0, pow(sigmaStr, 2)}};
double dcmBJ[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, sunEstB[3] = {0, 0, 0},
sunEstB[3] = {0,0,0}, magEstB[3] = {0,0,0}; magEstB[3] = {0, 0, 0};
QuaternionOperations::toDcm(propagatedQuaternion, dcmBJ); QuaternionOperations::toDcm(propagatedQuaternion, dcmBJ);
MatrixOperations<double>::multiply(*dcmBJ, normSunJ, sunEstB, 3, 3, 1); MatrixOperations<double>::multiply(*dcmBJ, normSunJ, sunEstB, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmBJ, normMagJ, magEstB, 3, 3, MatrixOperations<double>::multiply(*dcmBJ, normMagJ, magEstB, 3, 3, 1);
1); double quatEstErr[3] = {0, 0, 0};
double quatEstErr[3] = {0,0,0};
//Measurement Sensitivity Matrix // Measurement Sensitivity Matrix
double measSensMatrix11[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; // ss double measSensMatrix11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // ss
double measSensMatrix22[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; // mag double measSensMatrix22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // mag
double measSensMatrix33[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; // str double measSensMatrix33[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; // str
MathOperations<double>::skewMatrix(sunEstB, *measSensMatrix11); MathOperations<double>::skewMatrix(sunEstB, *measSensMatrix11);
MathOperations<double>::skewMatrix(magEstB, *measSensMatrix22); MathOperations<double>::skewMatrix(magEstB, *measSensMatrix22);
double measVecQuat[3] = {0,0,0}; double measVecQuat[3] = {0, 0, 0};
if (*validSTR_){ if (*validSTR_) {
double quatError[4] = {0,0,0,0}; double quatError[4] = {0, 0, 0, 0};
double invPropagatedQuat[4] = {0,0,0,0}; double invPropagatedQuat[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(propagatedQuaternion, invPropagatedQuat); QuaternionOperations::inverse(propagatedQuaternion, invPropagatedQuat);
QuaternionOperations::multiply(quaternionSTR, invPropagatedQuat, quatError); QuaternionOperations::multiply(quaternionSTR, invPropagatedQuat, quatError);
for (int i = 0 ; i < 3;i++){ for (int i = 0; i < 3; i++) {
measVecQuat[i]=2*quatError[i]/quatError[3]; measVecQuat[i] = 2 * quatError[i] / quatError[3];
} }
} }
// Adjust matrices based on valid measurements // Adjust matrices based on valid measurements
double measSensMatrix[MDF][6], measCovMatrix[MDF][MDF], measVec[MDF], double measSensMatrix[MDF][6], measCovMatrix[MDF][MDF], measVec[MDF], measEstVec[MDF];
measEstVec[MDF]; if (sensorsAvail == 1) { // All measurements valid
if (sensorsAvail==1){ //All measurements valid
measSensMatrix[0][0] = measSensMatrix11[0][0]; measSensMatrix[0][0] = measSensMatrix11[0][0];
measSensMatrix[0][1] = measSensMatrix11[0][1]; measSensMatrix[0][1] = measSensMatrix11[0][1];
measSensMatrix[0][2] = measSensMatrix11[0][2]; measSensMatrix[0][2] = measSensMatrix11[0][2];
@ -506,8 +458,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
measEstVec[7] = quatEstErr[1]; measEstVec[7] = quatEstErr[1];
measEstVec[8] = quatEstErr[2]; measEstVec[8] = quatEstErr[2];
} } else if (sensorsAvail == 2) { // ss / mag valid
else if(sensorsAvail==2){ // ss / mag valid
measSensMatrix[0][0] = measSensMatrix11[0][0]; measSensMatrix[0][0] = measSensMatrix11[0][0];
measSensMatrix[0][1] = measSensMatrix11[0][1]; measSensMatrix[0][1] = measSensMatrix11[0][1];
measSensMatrix[0][2] = measSensMatrix11[0][2]; measSensMatrix[0][2] = measSensMatrix11[0][2];
@ -596,8 +547,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
measEstVec[4] = magEstB[1]; measEstVec[4] = magEstB[1];
measEstVec[5] = magEstB[2]; measEstVec[5] = magEstB[2];
} } else if (sensorsAvail == 3) { // ss / str valid
else if(sensorsAvail==3){ // ss / str valid
measSensMatrix[0][0] = measSensMatrix11[0][0]; measSensMatrix[0][0] = measSensMatrix11[0][0];
measSensMatrix[0][1] = measSensMatrix11[0][1]; measSensMatrix[0][1] = measSensMatrix11[0][1];
@ -687,8 +637,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
measEstVec[4] = quatEstErr[1]; measEstVec[4] = quatEstErr[1];
measEstVec[5] = quatEstErr[2]; measEstVec[5] = quatEstErr[2];
} } else if (sensorsAvail == 4) { // mag / str avail
else if (sensorsAvail == 4){ // mag / str avail
measSensMatrix[0][0] = measSensMatrix22[0][0]; measSensMatrix[0][0] = measSensMatrix22[0][0];
measSensMatrix[0][1] = measSensMatrix22[0][1]; measSensMatrix[0][1] = measSensMatrix22[0][1];
@ -778,8 +727,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
measEstVec[4] = quatEstErr[1]; measEstVec[4] = quatEstErr[1];
measEstVec[5] = quatEstErr[2]; measEstVec[5] = quatEstErr[2];
} } else if (sensorsAvail == 5) { // only ss
else if (sensorsAvail == 5){ // only ss
measSensMatrix[0][0] = measSensMatrix11[0][0]; measSensMatrix[0][0] = measSensMatrix11[0][0];
measSensMatrix[0][1] = measSensMatrix11[0][1]; measSensMatrix[0][1] = measSensMatrix11[0][1];
@ -818,8 +766,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
measEstVec[1] = sunEstB[1]; measEstVec[1] = sunEstB[1];
measEstVec[2] = sunEstB[2]; measEstVec[2] = sunEstB[2];
} } else if (sensorsAvail == 6) { // only mag
else if (sensorsAvail == 6){ // only mag
measSensMatrix[0][0] = measSensMatrix22[0][0]; measSensMatrix[0][0] = measSensMatrix22[0][0];
measSensMatrix[0][1] = measSensMatrix22[0][1]; measSensMatrix[0][1] = measSensMatrix22[0][1];
@ -858,8 +805,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
measEstVec[1] = magEstB[1]; measEstVec[1] = magEstB[1];
measEstVec[2] = magEstB[2]; measEstVec[2] = magEstB[2];
} } else if (sensorsAvail == 7) { // only str
else if (sensorsAvail == 7){ // only str
measSensMatrix[0][0] = measSensMatrix33[0][0]; measSensMatrix[0][0] = measSensMatrix33[0][0];
measSensMatrix[0][1] = measSensMatrix33[0][1]; measSensMatrix[0][1] = measSensMatrix33[0][1];
@ -897,70 +843,71 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
measEstVec[0] = quatEstErr[0]; measEstVec[0] = quatEstErr[0];
measEstVec[1] = quatEstErr[1]; measEstVec[1] = quatEstErr[1];
measEstVec[2] = quatEstErr[2]; measEstVec[2] = quatEstErr[2];
} }
// Kalman Gain: [K = P * H' / (H * P * H' + R)] // Kalman Gain: [K = P * H' / (H * P * H' + R)]
double kalmanGain[6][MDF] = {{0}}, kalmanGain1[6][MDF] = {{0}}; double kalmanGain[6][MDF] = {{0}}, kalmanGain1[6][MDF] = {{0}};
double measSensMatrixTrans[6][MDF], residualCov[MDF][MDF], double measSensMatrixTrans[6][MDF], residualCov[MDF][MDF], residualCov1[6][MDF];
residualCov1[6][MDF];
// H * P * H' // H * P * H'
MatrixOperations<double>::transpose(*measSensMatrix,*measSensMatrixTrans,6,MDF); MatrixOperations<double>::transpose(*measSensMatrix, *measSensMatrixTrans, 6, MDF);
MatrixOperations<double>::multiply(*initialCovarianceMatrix,*measSensMatrixTrans,*residualCov1,6,6,MDF); MatrixOperations<double>::multiply(*initialCovarianceMatrix, *measSensMatrixTrans, *residualCov1,
MatrixOperations<double>::multiply(*measSensMatrix,*residualCov1,*residualCov,MDF,6,MDF); 6, 6, MDF);
MatrixOperations<double>::multiply(*measSensMatrix, *residualCov1, *residualCov, MDF, 6, MDF);
// negative values, restrictions ? // negative values, restrictions ?
// (H * P * H' + R) // (H * P * H' + R)
MatrixOperations<double>::add(*residualCov, *measCovMatrix, *residualCov, MDF, MDF); MatrixOperations<double>::add(*residualCov, *measCovMatrix, *residualCov, MDF, MDF);
// <<INVERSE residualCov HIER>> // <<INVERSE residualCov HIER>>
double invResidualCov1[MDF] = {0}; double invResidualCov1[MDF] = {0};
double invResidualCov[MDF][MDF] = {{0}}; double invResidualCov[MDF][MDF] = {{0}};
int inversionFailed = CholeskyDecomposition<double>::invertCholesky(*residualCov,*invResidualCov,invResidualCov1,MDF); int inversionFailed = CholeskyDecomposition<double>::invertCholesky(*residualCov, *invResidualCov,
if(inversionFailed) invResidualCov1, MDF);
{ if (inversionFailed) {
validMekf = false; validMekf = false;
return KALMAN_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed return KALMAN_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed
} }
// [K = P * H' / (H * P * H' + R)] // [K = P * H' / (H * P * H' + R)]
MatrixOperations<double>::multiply(*measSensMatrixTrans,*invResidualCov,*kalmanGain1,6,MDF,MDF); MatrixOperations<double>::multiply(*measSensMatrixTrans, *invResidualCov, *kalmanGain1, 6, MDF,
MatrixOperations<double>::multiply(*initialCovarianceMatrix,*kalmanGain1,*kalmanGain,6,6,MDF); MDF);
MatrixOperations<double>::multiply(*initialCovarianceMatrix, *kalmanGain1, *kalmanGain, 6, 6,
MDF);
/* ------- UPDATE -STEP ---------*/ /* ------- UPDATE -STEP ---------*/
// Update Covariance Matrix: P_plus = (I-K*H)*P_min // Update Covariance Matrix: P_plus = (I-K*H)*P_min
double covMatPlus[6][6] = {{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}, double covMatPlus[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}}; {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
double identityMatrix[6][6] = {{1,0,0,0,0,0},{0,1,0,0,0,0},{0,0,1,0,0,0}, double identityMatrix[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}}; {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
double updateCov1[6][6] = {{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}, double updateCov1[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}}; {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
MatrixOperations<double>::multiply(*kalmanGain,*measSensMatrix,*updateCov1,6,MDF,MDF); MatrixOperations<double>::multiply(*kalmanGain, *measSensMatrix, *updateCov1, 6, MDF, MDF);
MatrixOperations<double>::subtract(*identityMatrix,*updateCov1,*updateCov1,6,6); MatrixOperations<double>::subtract(*identityMatrix, *updateCov1, *updateCov1, 6, 6);
MatrixOperations<double>::multiply(*updateCov1,*initialCovarianceMatrix,*covMatPlus,6,6,6); MatrixOperations<double>::multiply(*updateCov1, *initialCovarianceMatrix, *covMatPlus, 6, 6, 6);
// Error State Vector // Error State Vector
double errStateVec[6] = {0,0,0,0,0,0}; double errStateVec[6] = {0, 0, 0, 0, 0, 0};
double errStateVec1[MDF] = {0}; double errStateVec1[MDF] = {0};
VectorOperations<double>::subtract(measVec,measEstVec,errStateVec1,MDF); VectorOperations<double>::subtract(measVec, measEstVec, errStateVec1, MDF);
MatrixOperations<double>::multiply(*kalmanGain,errStateVec1,errStateVec,6,MDF,1); MatrixOperations<double>::multiply(*kalmanGain, errStateVec1, errStateVec, 6, MDF, 1);
double errStateQuat[3] = {errStateVec[0], errStateVec[1], errStateVec[2]}; double errStateQuat[3] = {errStateVec[0], errStateVec[1], errStateVec[2]};
double errStateGyroBias[3] = {errStateVec[3], errStateVec[4], errStateVec[5]}; double errStateGyroBias[3] = {errStateVec[3], errStateVec[4], errStateVec[5]};
// State Vector Elements // State Vector Elements
double xi1[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, double xi1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
xi2[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; xi2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::skewMatrix(propagatedQuaternion, *xi2); MathOperations<double>::skewMatrix(propagatedQuaternion, *xi2);
double identityMatrix3[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; double identityMatrix3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
MatrixOperations<double>::multiplyScalar(*identityMatrix3, propagatedQuaternion[3], *xi1, 3, 3); MatrixOperations<double>::multiplyScalar(*identityMatrix3, propagatedQuaternion[3], *xi1, 3, 3);
MatrixOperations<double>::add(*xi1, *xi2, *xi1, 3, 3); MatrixOperations<double>::add(*xi1, *xi2, *xi1, 3, 3);
double xi[4][3] = {{xi1[0][0],xi1[0][1],xi1[0][2]}, double xi[4][3] = {
{xi1[1][0],xi1[1][1],xi1[1][2]}, {xi1[0][0], xi1[0][1], xi1[0][2]},
{xi1[2][0],xi1[2][1],xi1[2][2]}, {xi1[1][0], xi1[1][1], xi1[1][2]},
{-propagatedQuaternion[0],-propagatedQuaternion[1],-propagatedQuaternion[2]}}; {xi1[2][0], xi1[2][1], xi1[2][2]},
{-propagatedQuaternion[0], -propagatedQuaternion[1], -propagatedQuaternion[2]}};
double errQuatTerm[4] = {0,0,0,0}; double errQuatTerm[4] = {0, 0, 0, 0};
MatrixOperations<double>::multiply(*xi, errStateQuat, errQuatTerm, 4, 3, 1); MatrixOperations<double>::multiply(*xi, errStateQuat, errQuatTerm, 4, 3, 1);
VectorOperations<double>::mulScalar(errQuatTerm, 0.5, errQuatTerm, 4); VectorOperations<double>::mulScalar(errQuatTerm, 0.5, errQuatTerm, 4);
VectorOperations<double>::add(propagatedQuaternion, errQuatTerm, quatBJ, 4); VectorOperations<double>::add(propagatedQuaternion, errQuatTerm, quatBJ, 4);
@ -971,28 +918,27 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
outputQuat[2] = quatBJ[2]; outputQuat[2] = quatBJ[2];
outputQuat[3] = quatBJ[3]; outputQuat[3] = quatBJ[3];
double updatedGyroBias[3] = {0,0,0}; double updatedGyroBias[3] = {0, 0, 0};
VectorOperations<double>::add(biasRMU, errStateGyroBias, updatedGyroBias, 3); VectorOperations<double>::add(biasGYR, errStateGyroBias, updatedGyroBias, 3);
// Bias RMU State // Bias GYR State
biasRMU[0] = updatedGyroBias[0]; biasGYR[0] = updatedGyroBias[0];
biasRMU[1] = updatedGyroBias[1]; biasGYR[1] = updatedGyroBias[1];
biasRMU[2] = updatedGyroBias[2]; biasGYR[2] = updatedGyroBias[2];
/* ----------- PROPAGATION ----------*/ /* ----------- PROPAGATION ----------*/
//double sigmaU = kalmanFilterParameters->sensorNoiseBsRMU; // double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR;
//double sigmaV = kalmanFilterParameters->sensorNoiseArwRmu; // double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR;
double sigmaU = 3*3.141/180/3600; double sigmaU = 3 * 3.141 / 180 / 3600;
double sigmaV = 3*0.0043*3.141/sqrt(10)/180; double sigmaV = 3 * 0.0043 * 3.141 / sqrt(10) / 180;
double discTimeMatrix[6][6] = {{-1,0,0,0,0,0},{0,-1,0,0,0,0},{0,0,-1,0,0,0}, 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}}; {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
double rotRateEst[3] = {0,0,0}; double rotRateEst[3] = {0, 0, 0};
VectorOperations<double>::subtract(rateRMUs_, updatedGyroBias, rotRateEst, 3); VectorOperations<double>::subtract(rateGYRs_, updatedGyroBias, rotRateEst, 3);
double normRotEst =VectorOperations<double>::norm(rotRateEst, 3); double normRotEst = VectorOperations<double>::norm(rotRateEst, 3);
double crossRotEst[3][3] = {{0,-rotRateEst[2],rotRateEst[1]}, double crossRotEst[3][3] = {{0, -rotRateEst[2], rotRateEst[1]},
{rotRateEst[2],0,-rotRateEst[0]}, {rotRateEst[2], 0, -rotRateEst[0]},
{-rotRateEst[1],rotRateEst[0],0}}; {-rotRateEst[1], rotRateEst[0], 0}};
// Corrected Sat Rate via Bias // Corrected Sat Rate via Bias
outputSatRate[0] = rotRateEst[0]; outputSatRate[0] = rotRateEst[0];
@ -1000,19 +946,19 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
outputSatRate[2] = rotRateEst[2]; outputSatRate[2] = rotRateEst[2];
// Discrete Process Noise Covariance Q // Discrete Process Noise Covariance Q
double discProcessNoiseCov[6][6] = {{0,0,0,0,0,0},{0,0,0,0,0,0}, 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},{0,0,0,0,0,0}}; {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
double covQ1[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, double covQ1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
covQ2[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, covQ2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
covQ3[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, covQ3[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
transCovQ2[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; transCovQ2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
if(normRotEst*sampleTime< 3.141/10){ if (normRotEst * sampleTime < 3.141 / 10) {
double fact1 = sampleTime*pow(sigmaV,2)+pow(sampleTime,3)*pow(sigmaU,2/3); double fact1 = sampleTime * pow(sigmaV, 2) + pow(sampleTime, 3) * pow(sigmaU, 2 / 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact1, *covQ1, 3, 3); MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact1, *covQ1, 3, 3);
double fact2 = -(0.5*pow(sampleTime,2)*pow(sigmaU,2)); double fact2 = -(0.5 * pow(sampleTime, 2) * pow(sigmaU, 2));
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact2, *covQ2, 3, 3); MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact2, *covQ2, 3, 3);
MatrixOperations<double>::transpose(*covQ2, *transCovQ2, 3); MatrixOperations<double>::transpose(*covQ2, *transCovQ2, 3);
double fact3 = sampleTime*pow(sigmaU,2); double fact3 = sampleTime * pow(sigmaU, 2);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact3, *covQ3, 3, 3); MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact3, *covQ3, 3, 3);
discProcessNoiseCov[0][0] = covQ1[0][0]; discProcessNoiseCov[0][0] = covQ1[0][0];
@ -1051,34 +997,35 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
discProcessNoiseCov[5][3] = covQ3[2][0]; discProcessNoiseCov[5][3] = covQ3[2][0];
discProcessNoiseCov[5][4] = covQ3[2][1]; discProcessNoiseCov[5][4] = covQ3[2][1];
discProcessNoiseCov[5][5] = covQ3[2][2]; discProcessNoiseCov[5][5] = covQ3[2][2];
} } else {
else{ // double fact1 = sampleTime*pow(sigmaV,2);
//double fact1 = sampleTime*pow(sigmaV,2); double covQ11[3][3], covQ12[3][3], covQ13[3][3];
double covQ11[3][3],covQ12[3][3],covQ13[3][3]; // MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact1, *covQ1, 3, 3);
//MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact1, *covQ1, 3, 3); double fact1 = (2 * normRotEst + sampleTime - 2 * sin(normRotEst * sampleTime) -
double fact1 = (2*normRotEst+sampleTime-2*sin(normRotEst*sampleTime) pow(normRotEst, 3) / 3 * pow(sampleTime, 3)) /
-pow(normRotEst,3)/3*pow(sampleTime,3))/pow(normRotEst,5); pow(normRotEst, 5);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ11, 3, 3, 3); MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ11, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ11, fact1, *covQ11, 3, 3); MatrixOperations<double>::multiplyScalar(*covQ11, fact1, *covQ11, 3, 3);
double fact2 = pow(sampleTime,3); double fact2 = pow(sampleTime, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact2, *covQ12, 3, 3); MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact2, *covQ12, 3, 3);
MatrixOperations<double>::subtract(*covQ12, *covQ11, *covQ11, 3, 3); MatrixOperations<double>::subtract(*covQ12, *covQ11, *covQ11, 3, 3);
double fact3 = sampleTime*pow(sigmaV,2); double fact3 = sampleTime * pow(sigmaV, 2);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact3, *covQ13, 3, 3); MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact3, *covQ13, 3, 3);
MatrixOperations<double>::add(*covQ13, *covQ11, *covQ1, 3, 3); MatrixOperations<double>::add(*covQ13, *covQ11, *covQ1, 3, 3);
double covQ21[3][3], covQ22[3][3], covQ23[3][3]; double covQ21[3][3], covQ22[3][3], covQ23[3][3];
double fact4 = (0.5*pow(normRotEst,2)*pow(sampleTime,2) double fact4 =
+cos(normRotEst*sampleTime)-1)/pow(normRotEst,4); (0.5 * pow(normRotEst, 2) * pow(sampleTime, 2) + cos(normRotEst * sampleTime) - 1) /
pow(normRotEst, 4);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ21, 3, 3, 3); MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ21, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ21, fact4, *covQ21, 3, 3); MatrixOperations<double>::multiplyScalar(*covQ21, fact4, *covQ21, 3, 3);
double fact5 = 0.5*pow(sampleTime,2); double fact5 = 0.5 * pow(sampleTime, 2);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact5, *covQ22, 3, 3); MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact5, *covQ22, 3, 3);
MatrixOperations<double>::add(*covQ22, *covQ21, *covQ21, 3, 3); MatrixOperations<double>::add(*covQ22, *covQ21, *covQ21, 3, 3);
double fact6 = normRotEst*sampleTime-sin(normRotEst*sampleTime)/pow(normRotEst,3); double fact6 = normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3);
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact6, *covQ23, 3, 3); MatrixOperations<double>::multiplyScalar(*crossRotEst, fact6, *covQ23, 3, 3);
MatrixOperations<double>::subtract(*covQ23, *covQ21, *covQ21, 3, 3); MatrixOperations<double>::subtract(*covQ23, *covQ21, *covQ21, 3, 3);
double fact7 = pow(sigmaU,2); double fact7 = pow(sigmaU, 2);
MatrixOperations<double>::multiplyScalar(*covQ21, fact7, *covQ2, 3, 3); MatrixOperations<double>::multiplyScalar(*covQ21, fact7, *covQ2, 3, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact7, *covQ3, 3, 3); MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact7, *covQ3, 3, 3);
@ -1122,24 +1069,24 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
} }
// State Transition Matrix phi // State Transition Matrix phi
double phi1[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, double phi1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
phi2[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, phi2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
phi[6][6] = {{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}, phi[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
{0,0,0,1,0,0},{0,0,0,0,1,0},{0,0,0,0,0,1}}; {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
double phi11[3][3],phi12[3][3]; double phi11[3][3], phi12[3][3];
double fact1 = sin(normRotEst*sampleTime); double fact1 = sin(normRotEst * sampleTime);
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact1, *phi11, 3, 3); MatrixOperations<double>::multiplyScalar(*crossRotEst, fact1, *phi11, 3, 3);
double fact2 = (1-cos(normRotEst*sampleTime))/pow(normRotEst,2); double fact2 = (1 - cos(normRotEst * sampleTime)) / pow(normRotEst, 2);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi12, 3, 3, 3); MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi12, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*phi12, fact2, *phi12, 3, 3); MatrixOperations<double>::multiplyScalar(*phi12, fact2, *phi12, 3, 3);
MatrixOperations<double>::subtract(*identityMatrix3, *phi11, *phi11, 3, 3); MatrixOperations<double>::subtract(*identityMatrix3, *phi11, *phi11, 3, 3);
MatrixOperations<double>::add(*phi11, *phi12, *phi1, 3, 3); MatrixOperations<double>::add(*phi11, *phi12, *phi1, 3, 3);
double phi21[3][3],phi22[3][3]; double phi21[3][3], phi22[3][3];
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact2, *phi21, 3, 3); MatrixOperations<double>::multiplyScalar(*crossRotEst, fact2, *phi21, 3, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, sampleTime, *phi22, 3, 3); MatrixOperations<double>::multiplyScalar(*identityMatrix3, sampleTime, *phi22, 3, 3);
MatrixOperations<double>::subtract(*phi21, *phi22, *phi21, 3, 3); MatrixOperations<double>::subtract(*phi21, *phi22, *phi21, 3, 3);
double fact3 = (normRotEst*sampleTime-sin(normRotEst*sampleTime)/pow(normRotEst,3)); double fact3 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3));
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi22, 3, 3, 3); MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi22, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*phi22, fact3, *phi22, 3, 3); MatrixOperations<double>::multiplyScalar(*phi22, fact3, *phi22, 3, 3);
MatrixOperations<double>::subtract(*phi21, *phi22, *phi2, 3, 3); MatrixOperations<double>::subtract(*phi21, *phi22, *phi2, 3, 3);
@ -1164,10 +1111,9 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
phi[2][5] = phi2[2][2]; phi[2][5] = phi2[2][2];
// Propagated Quaternion // Propagated Quaternion
double rotSin[3] = {0,0,0}, double rotSin[3] = {0, 0, 0}, omega1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
omega1[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; double rotCos = cos(0.5 * normRotEst * sampleTime);
double rotCos = cos(0.5*normRotEst*sampleTime); double sinFac = sin(0.5 * normRotEst * sampleTime) / normRotEst;
double sinFac = sin(0.5*normRotEst*sampleTime)/normRotEst;
VectorOperations<double>::mulScalar(rotRateEst, sinFac, rotSin, 3); VectorOperations<double>::mulScalar(rotRateEst, sinFac, rotSin, 3);
double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -1175,15 +1121,14 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
MatrixOperations<double>::multiplyScalar(*identityMatrix3, rotCos, *omega1, 3, 3); MatrixOperations<double>::multiplyScalar(*identityMatrix3, rotCos, *omega1, 3, 3);
MatrixOperations<double>::subtract(*omega1, *skewSin, *omega1, 3, 3); MatrixOperations<double>::subtract(*omega1, *skewSin, *omega1, 3, 3);
double omega[4][4] = {{omega1[0][0],omega1[0][1],omega1[0][2],rotSin[0]}, double omega[4][4] = {{omega1[0][0], omega1[0][1], omega1[0][2], rotSin[0]},
{omega1[1][0],omega1[1][1],omega1[1][2],rotSin[1]}, {omega1[1][0], omega1[1][1], omega1[1][2], rotSin[1]},
{omega1[2][0],omega1[2][1],omega1[2][2],rotSin[2]}, {omega1[2][0], omega1[2][1], omega1[2][2], rotSin[2]},
{-rotSin[0],-rotSin[1],-rotSin[2],rotCos}}; {-rotSin[0], -rotSin[1], -rotSin[2], rotCos}};
MatrixOperations<double>::multiply(*omega, quatBJ, propagatedQuaternion, 4, 4, 1); MatrixOperations<double>::multiply(*omega, quatBJ, propagatedQuaternion, 4, 4, 1);
// Update Covariance Matrix // Update Covariance Matrix
double cov1[6][6], cov2[6][6], transDiscTimeMatrix[6][6], double cov1[6][6], cov2[6][6], transDiscTimeMatrix[6][6], transPhi[6][6];
transPhi[6][6];
MatrixOperations<double>::transpose(*discTimeMatrix, *transDiscTimeMatrix, 6); MatrixOperations<double>::transpose(*discTimeMatrix, *transDiscTimeMatrix, 6);
MatrixOperations<double>::multiply(*discProcessNoiseCov, *transDiscTimeMatrix, *cov1, 6, 6, 6); MatrixOperations<double>::multiply(*discProcessNoiseCov, *transDiscTimeMatrix, *cov1, 6, 6, 6);
MatrixOperations<double>::multiply(*discTimeMatrix, *cov1, *cov1, 6, 6, 6); MatrixOperations<double>::multiply(*discTimeMatrix, *cov1, *cov1, 6, 6, 6);
@ -1195,7 +1140,6 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
MatrixOperations<double>::add(*cov2, *cov1, *initialCovarianceMatrix, 6, 6); MatrixOperations<double>::add(*cov2, *cov1, *initialCovarianceMatrix, 6, 6);
validMekf = true; validMekf = true;
// Discrete Time Step // Discrete Time Step
// Check for new data in measurement -> SensorProcessing ? // Check for new data in measurement -> SensorProcessing ?

86
mission/controller/acs/MultiplicativeKalmanFilter.h
View File

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

8
mission/controller/acs/Navigation.cpp
View File

@ -36,8 +36,7 @@ void Navigation::useMekf(ACS::SensorValues *sensorValues, ACS::OutputValues *out
&outputValues->sunDirEstValid, outputValues->sunDirModel, &outputValues->sunDirModelValid, &outputValues->sunDirEstValid, outputValues->sunDirModel, &outputValues->sunDirModelValid,
outputValues->magFieldModel, &outputValues->magFieldModelValid, outputValues->quatMekfBJ, outputValues->magFieldModel, &outputValues->magFieldModelValid, outputValues->quatMekfBJ,
outputValues->satRateMekf); // VALIDS FOR QUAT AND RATE ?? outputValues->satRateMekf); // VALIDS FOR QUAT AND RATE ??
} } else {
else {
multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid, multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid,
outputValues->sunDirEst, &outputValues->sunDirEstValid, outputValues->sunDirEst, &outputValues->sunDirEstValid,
outputValues->sunDirModel, &outputValues->sunDirModelValid, outputValues->sunDirModel, &outputValues->sunDirModelValid,
@ -45,7 +44,8 @@ void Navigation::useMekf(ACS::SensorValues *sensorValues, ACS::OutputValues *out
kalmanInit = true; kalmanInit = true;
*mekfValid = 0; *mekfValid = 0;
// Maybe we need feedback from kalmanfilter to identify if there was a problem with the init // Maybe we need feedback from kalmanfilter to identify if there was a problem with the
//of kalman filter where does this class know from that kalman filter was not initialized ? // init of kalman filter where does this class know from that kalman filter was not
// initialized ?
} }
} }

4
mission/controller/acs/control/PtgCtrl.cpp
View File

@ -123,7 +123,7 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, doubl
} }
// calculating momentum of satellite and momentum of reaction wheels // calculating momentum of satellite and momentum of reaction wheels
double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3}; double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0}; double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4); VectorOperations<double>::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4);
MatrixOperations<double>::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4, MatrixOperations<double>::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4,
@ -145,7 +145,7 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, doubl
void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) { const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) {
double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3}; double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
double wheelMomentum[4] = {0, 0, 0, 0}; double wheelMomentum[4] = {0, 0, 0, 0};
double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60; double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60;
// Conversion to [rad/s] for further calculations // Conversion to [rad/s] for further calculations