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

Fix MEKF Inits #896

Merged
muellerr merged 6 commits from fix-mekf-inits into main 2024-05-29 10:12:36 +02:00
Conversation 0 Commits 6 Files Changed 4 +9 -9
1 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 6025ea5663 - Show all commits

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

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