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;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
double measSensMatrix[matrixDimensionFactor][6] = {{0}},
|
double measSensMatrix[9][6] {},
|
||||||
measCovMatrix[matrixDimensionFactor][matrixDimensionFactor] = {{0}},
|
measCovMatrix[9][9]{},
|
||||||
measVec[matrixDimensionFactor] = {0}, estVec[matrixDimensionFactor] = {0};
|
measVec[9] = {}, estVec[9] {};
|
||||||
kfUpdate(susData, mgmData, *measSensMatrix, *measCovMatrix, measVec, estVec);
|
kfUpdate(susData, mgmData, *measSensMatrix, *measCovMatrix, measVec, estVec);
|
||||||
|
|
||||||
double kalmanGain[6][matrixDimensionFactor] = {{0}};
|
double kalmanGain[6][9] = {};
|
||||||
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[6][matrixDimensionFactor] = {{0}},
|
double kalmanGainDen[6][9] {},
|
||||||
invKalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {{0}},
|
invKalmanGainDen[9][9] {},
|
||||||
residualCov[6][matrixDimensionFactor] = {{0}},
|
residualCov[6][9] {},
|
||||||
measSensMatrixTransposed[6][matrixDimensionFactor] = {{0}};
|
measSensMatrixTransposed[6][9]{};
|
||||||
|
|
||||||
MatrixOperations<double>::transpose(measSensMatrix, *measSensMatrixTransposed,
|
MatrixOperations<double>::transpose(measSensMatrix, *measSensMatrixTransposed,
|
||||||
matrixDimensionFactor, 6);
|
matrixDimensionFactor, 6);
|
||||||
|
|
Loading…
Reference in New Issue