eive/eive-obsw
eive/eive-obsw
12
3
Fork 0
Code Issues 4 Pull Requests 2 Releases 119 Wiki Activity

go home compiler, you're drunk
Some checks are pending
EIVE/eive-obsw/pipeline/head Build started...
Details

Browse Source
This commit is contained in:
Marius Eggert 2024-05-15 10:04:04 +02:00
parent 6025ea5663
commit 32271a98ff
1 changed files with 7 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -119,7 +119,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
measVec[matrixDimensionFactor] = {}, estVec[matrixDimensionFactor] = {};
kfUpdate(susData, mgmData, *measSensMatrix, *measCovMatrix, measVec, estVec);
double kalmanGain[6][matrixDimensionFactor] = {};
double kalmanGain[6][matrixDimensionFactor];
std::memset(kalmanGain, 0, sizeof(kalmanGain));
result = kfGain(*measSensMatrix, *measCovMatrix, *kalmanGain, attitudeEstimationData);
if (result != returnvalue::OK) {
reset(attitudeEstimationData);
@ -343,9 +344,10 @@ ReturnValue_t MultiplicativeKalmanFilter::kfGain(
acsctrl::AttitudeEstimationData *attitudeEstimationData) {
// Kalman Gain: K = P * H' / (H * P * H' + R)
double kalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {},
invKalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {},
residualCov[6][matrixDimensionFactor] = {},
measSensMatrixTransposed[6][matrixDimensionFactor] = {};
invKalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {};
double residualCov[6][matrixDimensionFactor], measSensMatrixTransposed[6][matrixDimensionFactor];
std::memset(residualCov, 0, sizeof(residualCov));
std::memset(measSensMatrixTransposed, 0, sizeof(measSensMatrixTransposed));
MatrixOperations<double>::transpose(measSensMatrix, *measSensMatrixTransposed,
matrixDimensionFactor, 6);
@ -382,8 +384,7 @@ void MultiplicativeKalmanFilter::kfCovAposteriori(double *kalmanGain, double *me
void MultiplicativeKalmanFilter::kfStateAposteriori(double *kalmanGain, double *measVec,
double *estVec) {
double stateVecErr[6] = {0, 0, 0, 0, 0, 0};
double plantOutputDiff[matrixDimensionFactor] = {};
double stateVecErr[6] = {0, 0, 0, 0, 0, 0}, plantOutputDiff[matrixDimensionFactor] = {};
VectorOperations<double>::subtract(measVec, estVec, plantOutputDiff, matrixDimensionFactor);
MatrixOperations<double>::multiply(kalmanGain, plantOutputDiff, stateVecErr, 6,
matrixDimensionFactor, 1);