From c50f8c716fad875314c94c0c01018b810c5c72fe Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 19 Oct 2022 11:01:27 +0200 Subject: [PATCH] amended GYR sensor fusion. replaced MGM sensor fusion --- mission/controller/acs/SensorProcessing.cpp | 104 ++++++++++---------- 1 file changed, 50 insertions(+), 54 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index a88d25ef..a6571496 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -42,9 +42,8 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const 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}, 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) { VectorOperations::subtract(mgm0Value, mgmParameters->mgm0hardIronOffset, mgm0ValueNoBias, 3); @@ -52,8 +51,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const mgm0ValueCalib, 3, 3, 1); MatrixOperations::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0ValueCalib, mgm0ValueBody, 3, 3, 1); - validCount += 1; - validUnit[0] = true; + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += mgm0ValueBody[i] / mgmParameters->mgm02variance[i]; + sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i]; + } } if (mgm1valid) { VectorOperations::subtract(mgm1Value, mgmParameters->mgm1hardIronOffset, mgm1ValueNoBias, @@ -62,8 +63,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const mgm1ValueCalib, 3, 3, 1); MatrixOperations::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1ValueCalib, mgm1ValueBody, 3, 3, 1); - validCount += 1; - validUnit[1] = true; + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += mgm1ValueBody[i] / mgmParameters->mgm13variance[i]; + sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i]; + } } if (mgm2valid) { VectorOperations::subtract(mgm2Value, mgmParameters->mgm2hardIronOffset, mgm2ValueNoBias, @@ -72,8 +75,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const mgm2ValueCalib, 3, 3, 1); MatrixOperations::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2ValueCalib, mgm2ValueBody, 3, 3, 1); - validCount += 1; - validUnit[2] = true; + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += mgm2ValueBody[i] / mgmParameters->mgm02variance[i]; + sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i]; + } } if (mgm3valid) { VectorOperations::subtract(mgm3Value, mgmParameters->mgm3hardIronOffset, mgm3ValueNoBias, @@ -82,8 +87,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const mgm3ValueCalib, 3, 3, 1); MatrixOperations::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3ValueCalib, mgm3ValueBody, 3, 3, 1); - validCount += 1; - validUnit[3] = true; + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += mgm3ValueBody[i] / mgmParameters->mgm13variance[i]; + sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i]; + } } if (mgm4valid) { VectorOperations::subtract(mgm4Value, mgmParameters->mgm4hardIronOffset, mgm4ValueNoBias, @@ -92,33 +99,17 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const mgm4ValueCalib, 3, 3, 1); MatrixOperations::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueCalib, mgm4ValueBody, 3, 3, 1); - validCount += 1; - validUnit[4] = true; - } - - /* -------- 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; + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += mgm4ValueBody[i] / mgmParameters->mgm4variance[i]; + sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i]; } } - // Selection Sort - double mgmValidValuesSort[3][validCount]; - MathOperations::selectionSort(*mgmValidValues, *mgmValidValuesSort, 3, validCount); - - uint8_t n = ceil(validCount / 2); - magFieldEst[0] = mgmValidValuesSort[0][n]; - magFieldEst[1] = mgmValidValuesSort[1][n]; - magFieldEst[2] = mgmValidValuesSort[2][n]; + for (uint8_t i = 0; i < 3; i++) { + magFieldEst[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i]; + } + sif::debug << "magFeildEst: " << magFieldEst[0] << " , " << magFieldEst[1] << " , " + << magFieldEst[2] << std::endl; + ; validMagField = true; //-----------------------Mag Rate Computation --------------------------------------------------- @@ -434,26 +425,31 @@ void SensorProcessing::processGyr( VectorOperations::add(gyr1ValueBody, gyr3ValueBody, gyr13ValuesSum, 3); VectorOperations::mulScalar(gyr13ValuesSum, .5, gyr13ValuesMean, 3); if (validUnit[0]) { - satRatEst[0] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight * - gyr13ValuesMean[0] + - gyrParameters->gyrFusionWeight * gyr0ValueBody[0]; - satRatEst[1] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight * - gyr13ValuesMean[1] + - gyrParameters->gyrFusionWeight * gyr0ValueBody[1]; - satRatEst[2] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight * - gyr13ValuesMean[2] + - gyrParameters->gyrFusionWeight * gyr0ValueBody[2]; - + satRatEst[0] = + ((gyr0ValueBody[0] / gyrParameters->gyr02variance[0]) + + (gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) / + ((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0])); + satRatEst[1] = + ((gyr0ValueBody[1] / gyrParameters->gyr02variance[1]) + + (gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) / + ((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1])); + satRatEst[2] = + ((gyr0ValueBody[2] / gyrParameters->gyr02variance[2]) + + (gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) / + ((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2])); } else if (validUnit[2]) { - satRatEst[0] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight * - gyr13ValuesMean[0] + - gyrParameters->gyrFusionWeight * gyr2ValueBody[0]; - satRatEst[1] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight * - gyr13ValuesMean[1] + - gyrParameters->gyrFusionWeight * gyr2ValueBody[1]; - satRatEst[2] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight * - gyr13ValuesMean[2] + - gyrParameters->gyrFusionWeight * gyr2ValueBody[2]; + satRatEst[0] = + ((gyr2ValueBody[0] / gyrParameters->gyr02variance[0]) + + (gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) / + ((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0])); + satRatEst[1] = + ((gyr2ValueBody[1] / gyrParameters->gyr02variance[1]) + + (gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) / + ((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1])); + satRatEst[2] = + ((gyr2ValueBody[2] / gyrParameters->gyr02variance[2]) + + (gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) / + ((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2])); } else satRatEst = gyr13ValuesMean; } else if (validUnit[1]) {