test
EIVE/eive-obsw/pipeline/head This commit looks good
Details
EIVE/eive-obsw/pipeline/head This commit looks good
Details
This commit is contained in:
parent
9a8c059e05
commit
777e147daa
|
@ -114,12 +114,12 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
|
|||
return result;
|
||||
}
|
||||
|
||||
double measSensMatrix[matrixDimensionFactor][6] = {{0}},
|
||||
measCovMatrix[matrixDimensionFactor][matrixDimensionFactor] = {{0}},
|
||||
measVec[matrixDimensionFactor] = {0}, estVec[matrixDimensionFactor] = {0};
|
||||
double measSensMatrix[9][6] {},
|
||||
measCovMatrix[9][9]{},
|
||||
measVec[9] = {}, estVec[9] {};
|
||||
kfUpdate(susData, mgmData, *measSensMatrix, *measCovMatrix, measVec, estVec);
|
||||
|
||||
double kalmanGain[6][matrixDimensionFactor] = {{0}};
|
||||
double kalmanGain[6][9] = {};
|
||||
result = kfGain(*measSensMatrix, *measCovMatrix, *kalmanGain, attitudeEstimationData);
|
||||
if (result != returnvalue::OK) {
|
||||
reset(attitudeEstimationData);
|
||||
|
@ -342,10 +342,10 @@ ReturnValue_t MultiplicativeKalmanFilter::kfGain(
|
|||
double *measSensMatrix, double *measCovMatrix, double *kalmanGain,
|
||||
acsctrl::AttitudeEstimationData *attitudeEstimationData) {
|
||||
// Kalman Gain: K = P * H' / (H * P * H' + R)
|
||||
double kalmanGainDen[6][matrixDimensionFactor] = {{0}},
|
||||
invKalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {{0}},
|
||||
residualCov[6][matrixDimensionFactor] = {{0}},
|
||||
measSensMatrixTransposed[6][matrixDimensionFactor] = {{0}};
|
||||
double kalmanGainDen[6][9] {},
|
||||
invKalmanGainDen[9][9] {},
|
||||
residualCov[6][9] {},
|
||||
measSensMatrixTransposed[6][9]{};
|
||||
|
||||
MatrixOperations<double>::transpose(measSensMatrix, *measSensMatrixTransposed,
|
||||
matrixDimensionFactor, 6);
|
||||
|
|
Loading…
Reference in New Issue