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

amended GYR sensor fusion. replaced MGM sensor fusion

Browse Source
This commit is contained in:
Marius Eggert 2022-10-19 11:01:27 +02:00
parent 86450f9d66
commit c50f8c716f
1 changed files with 50 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
mission/controller/acs/SensorProcessing.cpp
View File

@ -42,9 +42,8 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
mgm3ValueCalib[3] = {0, 0, 0}, mgm4ValueCalib[3] = {0, 0, 0}; mgm3ValueCalib[3] = {0, 0, 0}, mgm4ValueCalib[3] = {0, 0, 0};
float mgm0ValueBody[3] = {0, 0, 0}, mgm1ValueBody[3] = {0, 0, 0}, mgm2ValueBody[3] = {0, 0, 0}, float mgm0ValueBody[3] = {0, 0, 0}, mgm1ValueBody[3] = {0, 0, 0}, mgm2ValueBody[3] = {0, 0, 0},
mgm3ValueBody[3] = {0, 0, 0}, mgm4ValueBody[3] = {0, 0, 0}; mgm3ValueBody[3] = {0, 0, 0}, mgm4ValueBody[3] = {0, 0, 0};
float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0};
bool validUnit[5] = {false, false, false, false, false};
uint8_t validCount = 0;
if (mgm0valid) { if (mgm0valid) {
VectorOperations<float>::subtract(mgm0Value, mgmParameters->mgm0hardIronOffset, mgm0ValueNoBias, VectorOperations<float>::subtract(mgm0Value, mgmParameters->mgm0hardIronOffset, mgm0ValueNoBias,
3); 3);
@ -52,8 +51,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
mgm0ValueCalib, 3, 3, 1); mgm0ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0ValueCalib, MatrixOperations<float>::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0ValueCalib,
mgm0ValueBody, 3, 3, 1); mgm0ValueBody, 3, 3, 1);
validCount += 1; for (uint8_t i = 0; i < 3; i++) {
validUnit[0] = true; sensorFusionNumerator[i] += mgm0ValueBody[i] / mgmParameters->mgm02variance[i];
sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i];
}
} }
if (mgm1valid) { if (mgm1valid) {
VectorOperations<float>::subtract(mgm1Value, mgmParameters->mgm1hardIronOffset, mgm1ValueNoBias, VectorOperations<float>::subtract(mgm1Value, mgmParameters->mgm1hardIronOffset, mgm1ValueNoBias,
@ -62,8 +63,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
mgm1ValueCalib, 3, 3, 1); mgm1ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1ValueCalib, MatrixOperations<float>::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1ValueCalib,
mgm1ValueBody, 3, 3, 1); mgm1ValueBody, 3, 3, 1);
validCount += 1; for (uint8_t i = 0; i < 3; i++) {
validUnit[1] = true; sensorFusionNumerator[i] += mgm1ValueBody[i] / mgmParameters->mgm13variance[i];
sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
}
} }
if (mgm2valid) { if (mgm2valid) {
VectorOperations<float>::subtract(mgm2Value, mgmParameters->mgm2hardIronOffset, mgm2ValueNoBias, VectorOperations<float>::subtract(mgm2Value, mgmParameters->mgm2hardIronOffset, mgm2ValueNoBias,
@ -72,8 +75,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
mgm2ValueCalib, 3, 3, 1); mgm2ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2ValueCalib, MatrixOperations<float>::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2ValueCalib,
mgm2ValueBody, 3, 3, 1); mgm2ValueBody, 3, 3, 1);
validCount += 1; for (uint8_t i = 0; i < 3; i++) {
validUnit[2] = true; sensorFusionNumerator[i] += mgm2ValueBody[i] / mgmParameters->mgm02variance[i];
sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i];
}
} }
if (mgm3valid) { if (mgm3valid) {
VectorOperations<float>::subtract(mgm3Value, mgmParameters->mgm3hardIronOffset, mgm3ValueNoBias, VectorOperations<float>::subtract(mgm3Value, mgmParameters->mgm3hardIronOffset, mgm3ValueNoBias,
@ -82,8 +87,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
mgm3ValueCalib, 3, 3, 1); mgm3ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3ValueCalib, MatrixOperations<float>::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3ValueCalib,
mgm3ValueBody, 3, 3, 1); mgm3ValueBody, 3, 3, 1);
validCount += 1; for (uint8_t i = 0; i < 3; i++) {
validUnit[3] = true; sensorFusionNumerator[i] += mgm3ValueBody[i] / mgmParameters->mgm13variance[i];
sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
}
} }
if (mgm4valid) { if (mgm4valid) {
VectorOperations<float>::subtract(mgm4Value, mgmParameters->mgm4hardIronOffset, mgm4ValueNoBias, VectorOperations<float>::subtract(mgm4Value, mgmParameters->mgm4hardIronOffset, mgm4ValueNoBias,
@ -92,33 +99,17 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
mgm4ValueCalib, 3, 3, 1); mgm4ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueCalib, MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueCalib,
mgm4ValueBody, 3, 3, 1); mgm4ValueBody, 3, 3, 1);
validCount += 1; for (uint8_t i = 0; i < 3; i++) {
validUnit[4] = true; sensorFusionNumerator[i] += mgm4ValueBody[i] / mgmParameters->mgm4variance[i];
} sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i];
/* -------- MagFieldEst: Middle Value ------- */
float mgmValues[3][5] = {
{mgm0ValueBody[0], mgm1ValueBody[0], mgm2ValueBody[0], mgm3ValueBody[0], mgm4ValueBody[0]},
{mgm0ValueBody[1], mgm1ValueBody[1], mgm2ValueBody[1], mgm3ValueBody[1], mgm4ValueBody[1]},
{mgm0ValueBody[2], mgm1ValueBody[2], mgm2ValueBody[2], mgm3ValueBody[2], mgm4ValueBody[2]}};
double mgmValidValues[3][validCount];
uint8_t j = 0;
for (uint8_t i = 0; i < validCount; i++) {
if (validUnit[i]) {
mgmValidValues[0][j] = mgmValues[0][i];
mgmValidValues[1][j] = mgmValues[1][i];
mgmValidValues[2][j] = mgmValues[2][i];
j += 1;
} }
} }
// Selection Sort for (uint8_t i = 0; i < 3; i++) {
double mgmValidValuesSort[3][validCount]; magFieldEst[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i];
MathOperations<double>::selectionSort(*mgmValidValues, *mgmValidValuesSort, 3, validCount); }
sif::debug << "magFeildEst: " << magFieldEst[0] << " , " << magFieldEst[1] << " , "
uint8_t n = ceil(validCount / 2); << magFieldEst[2] << std::endl;
magFieldEst[0] = mgmValidValuesSort[0][n]; ;
magFieldEst[1] = mgmValidValuesSort[1][n];
magFieldEst[2] = mgmValidValuesSort[2][n];
validMagField = true; validMagField = true;
//-----------------------Mag Rate Computation --------------------------------------------------- //-----------------------Mag Rate Computation ---------------------------------------------------
@ -434,26 +425,31 @@ void SensorProcessing::processGyr(
VectorOperations<double>::add(gyr1ValueBody, gyr3ValueBody, gyr13ValuesSum, 3); VectorOperations<double>::add(gyr1ValueBody, gyr3ValueBody, gyr13ValuesSum, 3);
VectorOperations<double>::mulScalar(gyr13ValuesSum, .5, gyr13ValuesMean, 3); VectorOperations<double>::mulScalar(gyr13ValuesSum, .5, gyr13ValuesMean, 3);
if (validUnit[0]) { if (validUnit[0]) {
satRatEst[0] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight * satRatEst[0] =
gyr13ValuesMean[0] + ((gyr0ValueBody[0] / gyrParameters->gyr02variance[0]) +
gyrParameters->gyrFusionWeight * gyr0ValueBody[0]; (gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) /
satRatEst[1] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight * ((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0]));
gyr13ValuesMean[1] + satRatEst[1] =
gyrParameters->gyrFusionWeight * gyr0ValueBody[1]; ((gyr0ValueBody[1] / gyrParameters->gyr02variance[1]) +
satRatEst[2] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight * (gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) /
gyr13ValuesMean[2] + ((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1]));
gyrParameters->gyrFusionWeight * gyr0ValueBody[2]; satRatEst[2] =
((gyr0ValueBody[2] / gyrParameters->gyr02variance[2]) +
(gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) /
((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2]));
} else if (validUnit[2]) { } else if (validUnit[2]) {
satRatEst[0] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight * satRatEst[0] =
gyr13ValuesMean[0] + ((gyr2ValueBody[0] / gyrParameters->gyr02variance[0]) +
gyrParameters->gyrFusionWeight * gyr2ValueBody[0]; (gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) /
satRatEst[1] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight * ((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0]));
gyr13ValuesMean[1] + satRatEst[1] =
gyrParameters->gyrFusionWeight * gyr2ValueBody[1]; ((gyr2ValueBody[1] / gyrParameters->gyr02variance[1]) +
satRatEst[2] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight * (gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) /
gyr13ValuesMean[2] + ((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1]));
gyrParameters->gyrFusionWeight * gyr2ValueBody[2]; satRatEst[2] =
((gyr2ValueBody[2] / gyrParameters->gyr02variance[2]) +
(gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) /
((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2]));
} else } else
satRatEst = gyr13ValuesMean; satRatEst = gyr13ValuesMean;
} else if (validUnit[1]) { } else if (validUnit[1]) {