fixed int32_t to double warnings. reformats
This commit is contained in:
parent
0d3509b991
commit
8b23fd3dd2
@ -6,115 +6,97 @@
|
||||
*/
|
||||
|
||||
#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},
|
||||
{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;
|
||||
//AcsParameters mekfEstParams;
|
||||
//loadAcsParameters(&mekfEstParams);
|
||||
// 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 = 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
|
||||
VectorOperations<double>::normalize(sunDirJ, normSunJ, 3); //r1
|
||||
VectorOperations<double>::normalize(sunDir_, normSunB, 3); // b1
|
||||
VectorOperations<double>::normalize(magFieldJ, normMagJ, 3); // r2
|
||||
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(normSunB, normMagB, thirdAxis_B);
|
||||
VectorOperations<double>::normalize(thirdAxis_J, thirdAxis_J, 3); //rx
|
||||
VectorOperations<double>::normalize(thirdAxis_B, thirdAxis_B, 3); //bx
|
||||
VectorOperations<double>::normalize(thirdAxis_J, thirdAxis_J, 3); // rx
|
||||
VectorOperations<double>::normalize(thirdAxis_B, thirdAxis_B, 3); // bx
|
||||
|
||||
double weigthSun= 1 / (sigmaSun * sigmaSun); //a1
|
||||
double weigthMag= 1 / (sigmaMag * sigmaMag); //a2
|
||||
double weigthSun = 1 / (sigmaSun * sigmaSun); // a1
|
||||
double weigthMag = 1 / (sigmaMag * sigmaMag); // a2
|
||||
|
||||
double a[3] = {0,0,0}, b[3] = {0,0,0};
|
||||
VectorOperations<double>::mulScalar(normSunB, weigthSun, a, 3); //a
|
||||
VectorOperations<double>::mulScalar(normMagB, weigthMag, b, 3); //b
|
||||
double a[3] = {0, 0, 0}, b[3] = {0, 0, 0};
|
||||
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)
|
||||
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};
|
||||
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};
|
||||
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,55 +125,43 @@ 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}},
|
||||
matrixSunMag[3][3] = {{0,0,0},{0,0,0},{0,0,0}},
|
||||
matrixMagSun[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}},
|
||||
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);
|
||||
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);
|
||||
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(*partA, factor, *partA, 3, 3);
|
||||
MatrixOperations<double>::add(*partA, *matrixSun, *partA, 3, 3);
|
||||
MatrixOperations<double>::add(*partA, *matrixMag, *partA, 3, 3);
|
||||
|
||||
double crossSunMag[3] = {0,0,0};
|
||||
double identityMatrix3[3][3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } };
|
||||
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)}};
|
||||
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)}};
|
||||
initialCovarianceMatrix[0][0] = questCovMatrix[0][0];
|
||||
initialCovarianceMatrix[0][1] = questCovMatrix[0][1];
|
||||
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][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},
|
||||
// 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
|
||||
@ -246,58 +208,47 @@ 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
|
||||
//AcsParameters mekfEstParams;
|
||||
//loadAcsParameters(&mekfEstParams);
|
||||
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)){
|
||||
else if (!(*validSSModel) || !(*validMagModel)) {
|
||||
validMekf = false;
|
||||
return KALMAN_NO_MODEL;
|
||||
}
|
||||
// Check Measurements available from SS, MAG, STR
|
||||
if (*validSS && *validMagField_ && *validSTR_){
|
||||
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{
|
||||
sensorsAvail = 8; //no measurements
|
||||
} else {
|
||||
sensorsAvail = 8; // no measurements
|
||||
validMekf = false;
|
||||
return returnvalue::FAILED;
|
||||
}
|
||||
@ -308,47 +259,48 @@ 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
|
||||
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);
|
||||
double quatEstErr[3] = {0,0,0};
|
||||
MatrixOperations<double>::multiply(*dcmBJ, normMagJ, magEstB, 3, 3, 1);
|
||||
double quatEstErr[3] = {0, 0, 0};
|
||||
|
||||
//Measurement Sensitivity Matrix
|
||||
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 measSensMatrix33[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; // str
|
||||
// Measurement Sensitivity Matrix
|
||||
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 measSensMatrix33[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; // str
|
||||
MathOperations<double>::skewMatrix(sunEstB, *measSensMatrix11);
|
||||
MathOperations<double>::skewMatrix(magEstB, *measSensMatrix22);
|
||||
|
||||
double measVecQuat[3] = {0,0,0};
|
||||
if (*validSTR_){
|
||||
double quatError[4] = {0,0,0,0};
|
||||
double invPropagatedQuat[4] = {0,0,0,0};
|
||||
double measVecQuat[3] = {0, 0, 0};
|
||||
if (*validSTR_) {
|
||||
double quatError[4] = {0, 0, 0, 0};
|
||||
double invPropagatedQuat[4] = {0, 0, 0, 0};
|
||||
QuaternionOperations::inverse(propagatedQuaternion, invPropagatedQuat);
|
||||
QuaternionOperations::multiply(quaternionSTR, invPropagatedQuat, quatError);
|
||||
for (int i = 0 ; i < 3;i++){
|
||||
measVecQuat[i]=2*quatError[i]/quatError[3];
|
||||
for (int i = 0; i < 3; i++) {
|
||||
measVecQuat[i] = 2 * quatError[i] / quatError[3];
|
||||
}
|
||||
}
|
||||
// Adjust matrices based on valid measurements
|
||||
double measSensMatrix[MDF][6], measCovMatrix[MDF][MDF], measVec[MDF],
|
||||
measEstVec[MDF];
|
||||
if (sensorsAvail==1){ //All measurements valid
|
||||
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];
|
||||
measSensMatrix[0][2] = measSensMatrix11[0][2];
|
||||
@ -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,70 +843,71 @@ 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(*measSensMatrix,*residualCov1,*residualCov,MDF,6,MDF);
|
||||
MatrixOperations<double>::transpose(*measSensMatrix, *measSensMatrixTrans, 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)
|
||||
MatrixOperations<double>::add(*residualCov, *measCovMatrix, *residualCov, MDF, MDF);
|
||||
// <<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 ---------*/
|
||||
|
||||
// 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},
|
||||
{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},
|
||||
{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},
|
||||
{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>::subtract(*identityMatrix,*updateCov1,*updateCov1,6,6);
|
||||
MatrixOperations<double>::multiply(*updateCov1,*initialCovarianceMatrix,*covMatPlus,6,6,6);
|
||||
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}};
|
||||
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}};
|
||||
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}};
|
||||
MatrixOperations<double>::multiply(*kalmanGain, *measSensMatrix, *updateCov1, 6, MDF, MDF);
|
||||
MatrixOperations<double>::subtract(*identityMatrix, *updateCov1, *updateCov1, 6, 6);
|
||||
MatrixOperations<double>::multiply(*updateCov1, *initialCovarianceMatrix, *covMatPlus, 6, 6, 6);
|
||||
|
||||
// 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};
|
||||
VectorOperations<double>::subtract(measVec,measEstVec,errStateVec1,MDF);
|
||||
MatrixOperations<double>::multiply(*kalmanGain,errStateVec1,errStateVec,6,MDF,1);
|
||||
VectorOperations<double>::subtract(measVec, measEstVec, errStateVec1, MDF);
|
||||
MatrixOperations<double>::multiply(*kalmanGain, errStateVec1, errStateVec, 6, MDF, 1);
|
||||
|
||||
double errStateQuat[3] = {errStateVec[0], errStateVec[1], errStateVec[2]};
|
||||
double errStateGyroBias[3] = {errStateVec[3], errStateVec[4], errStateVec[5]};
|
||||
|
||||
// State Vector Elements
|
||||
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}};
|
||||
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}};
|
||||
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>::add(*xi1, *xi2, *xi1, 3, 3);
|
||||
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]}};
|
||||
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]}};
|
||||
|
||||
double errQuatTerm[4] = {0,0,0,0};
|
||||
double errQuatTerm[4] = {0, 0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*xi, errStateQuat, errQuatTerm, 4, 3, 1);
|
||||
VectorOperations<double>::mulScalar(errQuatTerm, 0.5, errQuatTerm, 4);
|
||||
VectorOperations<double>::add(propagatedQuaternion, errQuatTerm, quatBJ, 4);
|
||||
@ -971,28 +918,27 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
|
||||
outputQuat[2] = quatBJ[2];
|
||||
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];
|
||||
|
||||
double updatedGyroBias[3] = {0, 0, 0};
|
||||
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 = 3*3.141/180/3600;
|
||||
double sigmaV = 3*0.0043*3.141/sqrt(10)/180;
|
||||
// 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);
|
||||
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}};
|
||||
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(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}};
|
||||
|
||||
// Corrected Sat Rate via Bias
|
||||
outputSatRate[0] = rotRateEst[0];
|
||||
@ -1000,19 +946,19 @@ 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 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}},
|
||||
transCovQ2[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
|
||||
if(normRotEst*sampleTime< 3.141/10){
|
||||
double fact1 = sampleTime*pow(sigmaV,2)+pow(sampleTime,3)*pow(sigmaU,2/3);
|
||||
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}},
|
||||
transCovQ2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
if (normRotEst * sampleTime < 3.141 / 10) {
|
||||
double fact1 = sampleTime * pow(sigmaV, 2) + pow(sampleTime, 3) * pow(sigmaU, 2 / 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>::transpose(*covQ2, *transCovQ2, 3);
|
||||
double fact3 = sampleTime*pow(sigmaU,2);
|
||||
double fact3 = sampleTime * pow(sigmaU, 2);
|
||||
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact3, *covQ3, 3, 3);
|
||||
|
||||
discProcessNoiseCov[0][0] = covQ1[0][0];
|
||||
@ -1051,34 +997,35 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
|
||||
discProcessNoiseCov[5][3] = covQ3[2][0];
|
||||
discProcessNoiseCov[5][4] = covQ3[2][1];
|
||||
discProcessNoiseCov[5][5] = covQ3[2][2];
|
||||
}
|
||||
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);
|
||||
} 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);
|
||||
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ11, 3, 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>::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>::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);
|
||||
double fact5 = 0.5 * pow(sampleTime, 2);
|
||||
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact5, *covQ22, 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>::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(*identityMatrix3, fact7, *covQ3, 3, 3);
|
||||
@ -1122,24 +1069,24 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
|
||||
}
|
||||
|
||||
// State Transition Matrix phi
|
||||
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}},
|
||||
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}};
|
||||
double phi11[3][3],phi12[3][3];
|
||||
double fact1 = sin(normRotEst*sampleTime);
|
||||
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}},
|
||||
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}};
|
||||
double phi11[3][3], phi12[3][3];
|
||||
double fact1 = sin(normRotEst * sampleTime);
|
||||
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>::multiplyScalar(*phi12, fact2, *phi12, 3, 3);
|
||||
MatrixOperations<double>::subtract(*identityMatrix3, *phi11, *phi11, 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(*identityMatrix3, sampleTime, *phi22, 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>::multiplyScalar(*phi22, fact3, *phi22, 3, 3);
|
||||
MatrixOperations<double>::subtract(*phi21, *phi22, *phi2, 3, 3);
|
||||
@ -1164,10 +1111,9 @@ 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 rotCos = cos(0.5*normRotEst*sampleTime);
|
||||
double sinFac = sin(0.5*normRotEst*sampleTime)/normRotEst;
|
||||
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);
|
||||
|
||||
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>::subtract(*omega1, *skewSin, *omega1, 3, 3);
|
||||
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[2][0],omega1[2][1],omega1[2][2],rotSin[2]},
|
||||
{-rotSin[0],-rotSin[1],-rotSin[2],rotCos}};
|
||||
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[2][0], omega1[2][1], omega1[2][2], rotSin[2]},
|
||||
{-rotSin[0], -rotSin[1], -rotSin[2], rotCos}};
|
||||
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 ?
|
||||
|
||||
@ -7,22 +7,24 @@
|
||||
* @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"
|
||||
|
||||
class MultiplicativeKalmanFilter{
|
||||
public:
|
||||
class MultiplicativeKalmanFilter {
|
||||
public:
|
||||
/* @brief: Constructor
|
||||
* @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
|
||||
|
||||
/* @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,
|
||||
const double *magFieldJ,const bool *validMagModel);
|
||||
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);
|
||||
|
||||
private:
|
||||
/*Parameters*/
|
||||
AcsParameters::InertiaEIVE* inertiaEIVE;
|
||||
AcsParameters::KalmanFilterParameters* kalmanFilterParameters;
|
||||
private:
|
||||
/*Parameters*/
|
||||
AcsParameters::InertiaEIVE *inertiaEIVE;
|
||||
AcsParameters::KalmanFilterParameters *kalmanFilterParameters;
|
||||
double quaternion_STR_SB[4];
|
||||
bool validInit;
|
||||
double sampleTime = 0.1;
|
||||
|
||||
|
||||
/*States*/
|
||||
/*States*/
|
||||
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*/
|
||||
bool validMekf;
|
||||
uint8_t sensorsAvail;
|
||||
|
||||
/*Outputs*/
|
||||
/*Outputs*/
|
||||
double quatBJ[4]; /* Output Quaternion */
|
||||
double biasRMU[3]; /*Between measured and estimated sat Rate*/
|
||||
/*Parameter INIT*/
|
||||
//double alpha, gamma, beta;
|
||||
/*Functions*/
|
||||
double biasGYR[3]; /*Between measured and estimated sat Rate*/
|
||||
/*Parameter INIT*/
|
||||
// double alpha, gamma, beta;
|
||||
/*Functions*/
|
||||
void loadAcsParameters(AcsParameters *acsParameters_);
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */
|
||||
|
||||
@ -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 ?
|
||||
}
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user