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

First Version of ACS Controller #329

Merged
muellerr merged 106 commits from acs-ctrl-v1 into develop 2022-12-02 16:21:58 +01:00
Conversation 0 Commits 106 Files Changed 50 +1186 -1248
4 changed files with 1186 additions and 1248 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 8b23fd3dd2 - Show all commits

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

@ -6,41 +6,37 @@
*/
#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/QuaternionOperations.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*/
MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_) :
initialQuaternion { 0.5, 0.5, 0.5, 0.5 },initialCovarianceMatrix
{{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},
MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_)
: initialQuaternion{0.5, 0.5, 0.5, 0.5},
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}} {
loadAcsParameters(acsParameters_);
}
MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {
MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {}
}
void MultiplicativeKalmanFilter::loadAcsParameters(
AcsParameters *acsParameters_) {
void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_) {
inertiaEIVE = &(acsParameters_->inertiaEIVE);
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_, const bool *validSS,
const double *sunDirJ, const bool *validSSModel,
const double *magFieldJ,const bool *validMagModel) { // valids for "model measurements"?
void MultiplicativeKalmanFilter::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) { // valids for "model measurements"?
// check for valid mag/sun
if (*validMagField_ && *validSS && *validSSModel && *validMagModel) {
validInit = true;
@ -53,8 +49,8 @@ void MultiplicativeKalmanFilter::init(const double *magneticField_, const bool *
sigmaGyro = 0.1 * 3.141 / 180; // acs parameters
double normMagB[3] = {0,0,0}, normSunB[3] = {0,0,0},
normMagJ[3] = {0,0,0}, normSunJ[3] = {0,0,0};
double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
normSunJ[3] = {0, 0, 0};
VectorOperations<double>::normalize(magneticField_, normMagB, 3); // b2
VectorOperations<double>::normalize(sunDir_, normSunB, 3); // b1
VectorOperations<double>::normalize(magFieldJ, normMagJ, 3); // r2
@ -73,48 +69,34 @@ void MultiplicativeKalmanFilter::init(const double *magneticField_, const bool *
VectorOperations<double>::mulScalar(normSunB, weigthSun, a, 3); // a
VectorOperations<double>::mulScalar(normMagB, weigthMag, b, 3); // b
double thirdAxisCross[3] = {0,0,0}, weightACross[3] = {0,0,0},
weightBCross[3] = {0,0,0},weigthCrossSum[3] = {0,0,0};
double thirdAxisCross[3] = {0, 0, 0}, weightACross[3] = {0, 0, 0}, weightBCross[3] = {0, 0, 0},
weigthCrossSum[3] = {0, 0, 0};
VectorOperations<double>::cross(thirdAxis_B, thirdAxis_J,
thirdAxisCross); // cross(bx,rx)
VectorOperations<double>::cross(a, normSunJ, weightACross);
VectorOperations<double>::cross(b, normMagJ, weightBCross);
VectorOperations<double>::add(weightACross, weightBCross,
weigthCrossSum);
VectorOperations<double>::add(weightACross, weightBCross, weigthCrossSum);
double thirdAxisSum[3] = {0, 0, 0}, sum2[3] = {0, 0, 0};
VectorOperations<double>::add(thirdAxis_B, thirdAxis_J, thirdAxisSum);
VectorOperations<double>::add(weightACross, weightBCross, sum2);
double alpha = 0, beta = 0, gamma = 0, constPlus = 0, constMin = 0;
alpha = (1 + VectorOperations<double>::dot(thirdAxis_B, thirdAxis_J))
* (VectorOperations<double>::dot(a, normSunJ)
+ VectorOperations<double>::dot(b, normMagJ))
+ VectorOperations<double>::dot(thirdAxisCross, weigthCrossSum);
alpha = (1 + VectorOperations<double>::dot(thirdAxis_B, thirdAxis_J)) *
(VectorOperations<double>::dot(a, normSunJ) +
VectorOperations<double>::dot(b, normMagJ)) +
VectorOperations<double>::dot(thirdAxisCross, weigthCrossSum);
beta = VectorOperations<double>::dot(thirdAxisSum, sum2);
gamma = sqrt(alpha * alpha + beta * beta);
constPlus = 1
/ (2
* sqrt(
gamma * (gamma + alpha)
* (1
+ VectorOperations<double>::dot(
thirdAxis_B,
thirdAxis_J))));
constMin = 1
/ (2
* sqrt(
gamma * (gamma - alpha)
* (1
+ VectorOperations<double>::dot(
thirdAxis_B,
thirdAxis_J))));
constPlus = 1 / (2 * sqrt(gamma * (gamma + alpha) *
(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*/
double quat[3] = {0, 0, 0}, var1[3] = {0, 0, 0}, var2[3] = {0, 0, 0};
if (alpha >= 0) {
VectorOperations<double>::mulScalar(thirdAxisCross, gamma + alpha,
var1, 3);
VectorOperations<double>::mulScalar(thirdAxisCross, gamma + alpha, var1, 3);
VectorOperations<double>::add(thirdAxis_B, thirdAxis_J, var2, 3);
VectorOperations<double>::mulScalar(var2, beta, var2, 3);
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++) {
initialQuaternion[i] = quat[i];
}
initialQuaternion[3] = (gamma + alpha)
* (1
+ VectorOperations<double>::dot(thirdAxis_B,
thirdAxis_J));
VectorOperations<double>::mulScalar(initialQuaternion,
constPlus, initialQuaternion, 4);
initialQuaternion[3] =
(gamma + alpha) * (1 + VectorOperations<double>::dot(thirdAxis_B, thirdAxis_J));
VectorOperations<double>::mulScalar(initialQuaternion, constPlus, initialQuaternion, 4);
} else {
double diffGammaAlpha = gamma - alpha;
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++) {
initialQuaternion[i] = quat[i];
}
initialQuaternion[3] = beta
* (1
+ VectorOperations<double>::dot(thirdAxis_B,
thirdAxis_J));
VectorOperations<double>::mulScalar(initialQuaternion,
constMin, initialQuaternion, 4);
initialQuaternion[3] = beta * (1 + VectorOperations<double>::dot(thirdAxis_B, thirdAxis_J));
VectorOperations<double>::mulScalar(initialQuaternion, constMin, initialQuaternion, 4);
}
propagatedQuaternion[0] = initialQuaternion[0];
propagatedQuaternion[1] = initialQuaternion[1];
@ -151,12 +125,11 @@ void MultiplicativeKalmanFilter::init(const double *magneticField_, const bool *
propagatedQuaternion[3] = initialQuaternion[3];
/*Initial Covariance Matrix------------------------------------ */
double dcmBJ[3][3] = {{0,0,0},{0,0,0},{0,0,0}},
sunEstB[3] = {0,0,0}, magEstB[3] = {0,0,0};
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, sunEstB[3] = {0, 0, 0},
magEstB[3] = {0, 0, 0};
QuaternionOperations::toDcm(initialQuaternion, dcmBJ);
MatrixOperations<double>::multiply(*dcmBJ, normSunJ, sunEstB, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmBJ, normMagJ, magEstB, 3, 3,
1);
MatrixOperations<double>::multiply(*dcmBJ, normMagJ, magEstB, 3, 3, 1);
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}},
@ -169,14 +142,10 @@ void MultiplicativeKalmanFilter::init(const double *magneticField_, const bool *
MatrixOperations<double>::multiply(magEstB, sunEstB, *matrixMagSun, 3, 1, 3);
double partA[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, partB = 0;
MatrixOperations<double>::multiplyScalar(*matrixSun,
weigthSun * weigthSun, *matrixSun, 3, 3);
MatrixOperations<double>::multiplyScalar(*matrixMag,
weigthMag * weigthMag, *matrixMag, 3, 3);
MatrixOperations<double>::add(*matrixSunMag, *matrixMagSun, *partA, 3,
3);
double factor = weigthMag * weigthSun
* VectorOperations<double>::dot(sunEstB, magEstB);
MatrixOperations<double>::multiplyScalar(*matrixSun, weigthSun * weigthSun, *matrixSun, 3, 3);
MatrixOperations<double>::multiplyScalar(*matrixMag, weigthMag * weigthMag, *matrixMag, 3, 3);
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>::add(*partA, *matrixSun, *partA, 3, 3);
MatrixOperations<double>::add(*partA, *matrixMag, *partA, 3, 3);
@ -184,22 +153,15 @@ void MultiplicativeKalmanFilter::init(const double *magneticField_, const bool *
double crossSunMag[3] = {0, 0, 0};
double identityMatrix3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
VectorOperations<double>::cross(sunEstB, magEstB, crossSunMag);
partB =
1
/ (weigthMag * weigthSun
* pow(
VectorOperations<double>::norm(
crossSunMag, 3), 2));
partB = 1 / (weigthMag * weigthSun * pow(VectorOperations<double>::norm(crossSunMag, 3), 2));
MatrixOperations<double>::multiplyScalar(*partA, partB, *partA, 3, 3);
MatrixOperations<double>::add(*matrixSunMag, *identityMatrix3, *partA, 3,
3);
MatrixOperations<double>::add(*matrixSunMag, *identityMatrix3, *partA, 3, 3);
factor = 1 / (weigthMag + weigthMag);
double questCovMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiplyScalar(*partA, factor,
*questCovMatrix, 3, 3);
double initGyroCov[3][3] = {{pow(sigmaGyro,2),0,0},{0,pow(sigmaGyro,2),0},
{0,0,pow(sigmaGyro,2)}};
MatrixOperations<double>::multiplyScalar(*partA, factor, *questCovMatrix, 3, 3);
double initGyroCov[3][3] = {
{pow(sigmaGyro, 2), 0, 0}, {0, pow(sigmaGyro, 2), 0}, {0, 0, pow(sigmaGyro, 2)}};
initialCovarianceMatrix[0][0] = questCovMatrix[0][0];
initialCovarianceMatrix[0][1] = questCovMatrix[0][1];
initialCovarianceMatrix[0][2] = questCovMatrix[0][2];
@ -246,21 +208,17 @@ void MultiplicativeKalmanFilter::init(const double *magneticField_, const bool *
// --------------- MEKF DISCRETE TIME STEP -------------------------------
ReturnValue_t MultiplicativeKalmanFilter::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) {
// Check for RMU Measurements
const double *quaternionSTR, const bool *validSTR_, const double *rateGYRs_,
const bool *validGYRs_, 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) {
// Check for GYR Measurements
// AcsParameters mekfEstParams;
// loadAcsParameters(&mekfEstParams);
int MDF = 0; // Matrix Dimension Factor
if (!(*validRMUs_)){
if (!(*validGYRs_)) {
validMekf = false;
return KALMAN_NO_RMU_MEAS;
return KALMAN_NO_GYR_MEAS;
}
// Check for Model Calculations
else if (!(*validSSModel) || !(*validMagModel)) {
@ -271,32 +229,25 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
if (*validSS && *validMagField_ && *validSTR_) {
sensorsAvail = 1;
MDF = 9;
}
else if (*validSS && *validMagField_ && !(*validSTR_)){
} else if (*validSS && *validMagField_ && !(*validSTR_)) {
sensorsAvail = 2;
MDF = 6;
}
else if (*validSS && !(*validMagField_) && *validSTR_){
} else if (*validSS && !(*validMagField_) && *validSTR_) {
sensorsAvail = 3;
MDF = 6;
}
else if (!(*validSS) && *validMagField_ && *validSTR_){
} else if (!(*validSS) && *validMagField_ && *validSTR_) {
sensorsAvail = 4;
MDF = 6;
}
else if (*validSS && !(*validMagField_) && !(*validSTR_)){
} else if (*validSS && !(*validMagField_) && !(*validSTR_)) {
sensorsAvail = 5;
MDF = 3;
}
else if (!(*validSS) && *validMagField_ && !(*validSTR_)){
} else if (!(*validSS) && *validMagField_ && !(*validSTR_)) {
sensorsAvail = 6;
MDF = 3;
}
else if (!(*validSS) && !(*validMagField_) && *validSTR_){
} else if (!(*validSS) && !(*validMagField_) && *validSTR_) {
sensorsAvail = 7;
MDF = 3;
}
else{
} else {
sensorsAvail = 8; // no measurements
validMekf = false;
return returnvalue::FAILED;
@ -308,24 +259,26 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
sigmaMag = kalmanFilterParameters->sensorNoiseMAG;
sigmaStr = kalmanFilterParameters->sensorNoiseSTR;
double normMagB[3] = {0,0,0}, normSunB[3] = {0,0,0},
normMagJ[3] = {0,0,0}, normSunJ[3] = {0,0,0};
double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
normSunJ[3] = {0, 0, 0};
VectorOperations<double>::normalize(magneticField_, normMagB, 3); // b2
VectorOperations<double>::normalize(sunDir_, normSunB, 3); // b1
VectorOperations<double>::normalize(magFieldJ, normMagJ, 3); // r2
VectorOperations<double>::normalize(sunDirJ, normSunJ, 3); // r1
/*-------GAIN - UPDATE - STEP------*/
double covMatrix11[3][3] = {{pow(sigmaSun,2),0,0},{0,pow(sigmaSun,2),0},{0,0,pow(sigmaSun,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 covMatrix11[3][3] = {
{pow(sigmaSun, 2), 0, 0}, {0, pow(sigmaSun, 2), 0}, {0, 0, pow(sigmaSun, 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}},
sunEstB[3] = {0,0,0}, magEstB[3] = {0,0,0};
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, sunEstB[3] = {0, 0, 0},
magEstB[3] = {0, 0, 0};
QuaternionOperations::toDcm(propagatedQuaternion, dcmBJ);
MatrixOperations<double>::multiply(*dcmBJ, normSunJ, sunEstB, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmBJ, normMagJ, magEstB, 3, 3,
1);
MatrixOperations<double>::multiply(*dcmBJ, normMagJ, magEstB, 3, 3, 1);
double quatEstErr[3] = {0, 0, 0};
// Measurement Sensitivity Matrix
@ -346,8 +299,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
}
}
// Adjust matrices based on valid measurements
double measSensMatrix[MDF][6], measCovMatrix[MDF][MDF], measVec[MDF],
measEstVec[MDF];
double measSensMatrix[MDF][6], measCovMatrix[MDF][MDF], measVec[MDF], measEstVec[MDF];
if (sensorsAvail == 1) { // All measurements valid
measSensMatrix[0][0] = measSensMatrix11[0][0];
measSensMatrix[0][1] = measSensMatrix11[0][1];
@ -506,8 +458,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
measEstVec[7] = quatEstErr[1];
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][1] = measSensMatrix11[0][1];
measSensMatrix[0][2] = measSensMatrix11[0][2];
@ -596,8 +547,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
measEstVec[4] = magEstB[1];
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][1] = measSensMatrix11[0][1];
@ -687,8 +637,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
measEstVec[4] = quatEstErr[1];
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][1] = measSensMatrix22[0][1];
@ -778,8 +727,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
measEstVec[4] = quatEstErr[1];
measEstVec[5] = quatEstErr[2];
}
else if (sensorsAvail == 5){ // only ss
} else if (sensorsAvail == 5) { // only ss
measSensMatrix[0][0] = measSensMatrix11[0][0];
measSensMatrix[0][1] = measSensMatrix11[0][1];
@ -818,8 +766,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
measEstVec[1] = sunEstB[1];
measEstVec[2] = sunEstB[2];
}
else if (sensorsAvail == 6){ // only mag
} else if (sensorsAvail == 6) { // only mag
measSensMatrix[0][0] = measSensMatrix22[0][0];
measSensMatrix[0][1] = measSensMatrix22[0][1];
@ -858,8 +805,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
measEstVec[1] = magEstB[1];
measEstVec[2] = magEstB[2];
}
else if (sensorsAvail == 7){ // only str
} else if (sensorsAvail == 7) { // only str
measSensMatrix[0][0] = measSensMatrix33[0][0];
measSensMatrix[0][1] = measSensMatrix33[0][1];
@ -897,16 +843,15 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
measEstVec[0] = quatEstErr[0];
measEstVec[1] = quatEstErr[1];
measEstVec[2] = quatEstErr[2];
}
// Kalman Gain: [K = P * H' / (H * P * H' + R)]
double kalmanGain[6][MDF] = {{0}}, kalmanGain1[6][MDF] = {{0}};
double measSensMatrixTrans[6][MDF], residualCov[MDF][MDF],
residualCov1[6][MDF];
double measSensMatrixTrans[6][MDF], residualCov[MDF][MDF], residualCov1[6][MDF];
// H * P * H'
MatrixOperations<double>::transpose(*measSensMatrix, *measSensMatrixTrans, 6, MDF);
MatrixOperations<double>::multiply(*initialCovarianceMatrix,*measSensMatrixTrans,*residualCov1,6,6,MDF);
MatrixOperations<double>::multiply(*initialCovarianceMatrix, *measSensMatrixTrans, *residualCov1,
6, 6, MDF);
MatrixOperations<double>::multiply(*measSensMatrix, *residualCov1, *residualCov, MDF, 6, MDF);
// negative values, restrictions ?
// (H * P * H' + R)
@ -914,17 +859,18 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
// <<INVERSE residualCov HIER>>
double invResidualCov1[MDF] = {0};
double invResidualCov[MDF][MDF] = {{0}};
int inversionFailed = CholeskyDecomposition<double>::invertCholesky(*residualCov,*invResidualCov,invResidualCov1,MDF);
if(inversionFailed)
{
int inversionFailed = CholeskyDecomposition<double>::invertCholesky(*residualCov, *invResidualCov,
invResidualCov1, MDF);
if (inversionFailed) {
validMekf = false;
return KALMAN_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed
}
// [K = P * H' / (H * P * H' + R)]
MatrixOperations<double>::multiply(*measSensMatrixTrans,*invResidualCov,*kalmanGain1,6,MDF,MDF);
MatrixOperations<double>::multiply(*initialCovarianceMatrix,*kalmanGain1,*kalmanGain,6,6,MDF);
MatrixOperations<double>::multiply(*measSensMatrixTrans, *invResidualCov, *kalmanGain1, 6, MDF,
MDF);
MatrixOperations<double>::multiply(*initialCovarianceMatrix, *kalmanGain1, *kalmanGain, 6, 6,
MDF);
/* ------- UPDATE -STEP ---------*/
@ -955,7 +901,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
double identityMatrix3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
MatrixOperations<double>::multiplyScalar(*identityMatrix3, propagatedQuaternion[3], *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[0][0], xi1[0][1], xi1[0][2]},
{xi1[1][0], xi1[1][1], xi1[1][2]},
{xi1[2][0], xi1[2][1], xi1[2][2]},
{-propagatedQuaternion[0], -propagatedQuaternion[1], -propagatedQuaternion[2]}};
@ -972,23 +919,22 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
outputQuat[3] = quatBJ[3];
double updatedGyroBias[3] = {0, 0, 0};
VectorOperations<double>::add(biasRMU, errStateGyroBias, updatedGyroBias, 3);
// Bias RMU State
biasRMU[0] = updatedGyroBias[0];
biasRMU[1] = updatedGyroBias[1];
biasRMU[2] = updatedGyroBias[2];
VectorOperations<double>::add(biasGYR, errStateGyroBias, updatedGyroBias, 3);
// Bias GYR State
biasGYR[0] = updatedGyroBias[0];
biasGYR[1] = updatedGyroBias[1];
biasGYR[2] = updatedGyroBias[2];
/* ----------- PROPAGATION ----------*/
//double sigmaU = kalmanFilterParameters->sensorNoiseBsRMU;
//double sigmaV = kalmanFilterParameters->sensorNoiseArwRmu;
// 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 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};
VectorOperations<double>::subtract(rateRMUs_, updatedGyroBias, rotRateEst, 3);
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]},
@ -1000,8 +946,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
outputSatRate[2] = rotRateEst[2];
// 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}};
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}};
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}},
covQ3[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
@ -1051,13 +997,13 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
discProcessNoiseCov[5][3] = covQ3[2][0];
discProcessNoiseCov[5][4] = covQ3[2][1];
discProcessNoiseCov[5][5] = covQ3[2][2];
}
else{
} else {
// double fact1 = sampleTime*pow(sigmaV,2);
double covQ11[3][3], covQ12[3][3], covQ13[3][3];
// MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact1, *covQ1, 3, 3);
double fact1 = (2*normRotEst+sampleTime-2*sin(normRotEst*sampleTime)
-pow(normRotEst,3)/3*pow(sampleTime,3))/pow(normRotEst,5);
double fact1 = (2 * normRotEst + sampleTime - 2 * sin(normRotEst * sampleTime) -
pow(normRotEst, 3) / 3 * pow(sampleTime, 3)) /
pow(normRotEst, 5);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ11, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ11, fact1, *covQ11, 3, 3);
double fact2 = pow(sampleTime, 3);
@ -1068,8 +1014,9 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
MatrixOperations<double>::add(*covQ13, *covQ11, *covQ1, 3, 3);
double covQ21[3][3], covQ22[3][3], covQ23[3][3];
double fact4 = (0.5*pow(normRotEst,2)*pow(sampleTime,2)
+cos(normRotEst*sampleTime)-1)/pow(normRotEst,4);
double fact4 =
(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>::multiplyScalar(*covQ21, fact4, *covQ21, 3, 3);
double fact5 = 0.5 * pow(sampleTime, 2);
@ -1164,8 +1111,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
phi[2][5] = phi2[2][2];
// Propagated Quaternion
double rotSin[3] = {0,0,0},
omega1[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
double rotSin[3] = {0, 0, 0}, omega1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double rotCos = cos(0.5 * normRotEst * sampleTime);
double sinFac = sin(0.5 * normRotEst * sampleTime) / normRotEst;
VectorOperations<double>::mulScalar(rotRateEst, sinFac, rotSin, 3);
@ -1182,8 +1128,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
MatrixOperations<double>::multiply(*omega, quatBJ, propagatedQuaternion, 4, 4, 1);
// Update Covariance Matrix
double cov1[6][6], cov2[6][6], transDiscTimeMatrix[6][6],
transPhi[6][6];
double cov1[6][6], cov2[6][6], transDiscTimeMatrix[6][6], transPhi[6][6];
MatrixOperations<double>::transpose(*discTimeMatrix, *transDiscTimeMatrix, 6);
MatrixOperations<double>::multiply(*discProcessNoiseCov, *transDiscTimeMatrix, *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);
validMekf = true;
// Discrete Time Step
// Check for new data in measurement -> SensorProcessing ?

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

@ -7,16 +7,18 @@
* @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
* @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 "config/classIds.h"
#include <stdint.h> //uint8_t
#include <time.h> /*purpose, timeval ?*/
#include "config/classIds.h"
//#include <_timeval.h>
#include "AcsParameters.h"
@ -31,49 +33,44 @@ public:
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
/* @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,
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
/* @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
* rateGYRs_ Estimated satellite rotation rate from the
* Gyroscopes [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 ReturnValue_t Feedback of this class, KALMAN_NO_GYR_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);
ReturnValue_t mekfEst(const double *quaternionSTR, const bool *validSTR_, const double *rateGYRs_,
const bool *validGYRs_, 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
// 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_GYR_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);
@ -85,7 +82,6 @@ private:
bool validInit;
double sampleTime = 0.1;
/*States*/
double initialQuaternion[4]; /*after reset?QUEST*/
double initialCovarianceMatrix[6][6]; /*after reset?QUEST*/
@ -95,13 +91,11 @@ private:
/*Outputs*/
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*/
// double alpha, gamma, beta;
/*Functions*/
void loadAcsParameters(AcsParameters *acsParameters_);
};
#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->magFieldModel, &outputValues->magFieldModelValid, outputValues->quatMekfBJ,
outputValues->satRateMekf); // VALIDS FOR QUAT AND RATE ??
}
else {
} else {
multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid,
outputValues->sunDirEst, &outputValues->sunDirEstValid,
outputValues->sunDirModel, &outputValues->sunDirModelValid,
@ -45,7 +44,8 @@ void Navigation::useMekf(ACS::SensorValues *sensorValues, ACS::OutputValues *out
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 ?
// 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 ?
}
}

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
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};
VectorOperations<double>::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 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,
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 rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60;
// Conversion to [rad/s] for further calculations