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 +7 -6
1 changed files with 7 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 32271a98ff - Show all commits

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

@ -119,7 +119,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
measVec[matrixDimensionFactor] = {}, estVec[matrixDimensionFactor] = {}; measVec[matrixDimensionFactor] = {}, estVec[matrixDimensionFactor] = {};
kfUpdate(susData, mgmData, *measSensMatrix, *measCovMatrix, measVec, estVec); 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); result = kfGain(*measSensMatrix, *measCovMatrix, *kalmanGain, attitudeEstimationData);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
reset(attitudeEstimationData); reset(attitudeEstimationData);
@ -343,9 +344,10 @@ ReturnValue_t MultiplicativeKalmanFilter::kfGain(
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] = {}, double kalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {},
invKalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {}, invKalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {};
residualCov[6][matrixDimensionFactor] = {}, double residualCov[6][matrixDimensionFactor], measSensMatrixTransposed[6][matrixDimensionFactor];
measSensMatrixTransposed[6][matrixDimensionFactor] = {}; std::memset(residualCov, 0, sizeof(residualCov));
std::memset(measSensMatrixTransposed, 0, sizeof(measSensMatrixTransposed));
MatrixOperations<double>::transpose(measSensMatrix, *measSensMatrixTransposed, MatrixOperations<double>::transpose(measSensMatrix, *measSensMatrixTransposed,
matrixDimensionFactor, 6); matrixDimensionFactor, 6);
@ -382,8 +384,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}, plantOutputDiff[matrixDimensionFactor] = {};
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);