amended GYR sensor fusion. replaced MGM sensor fusion
This commit is contained in:
parent
86450f9d66
commit
c50f8c716f
|
|
@ -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<float>::subtract(mgm0Value, mgmParameters->mgm0hardIronOffset, mgm0ValueNoBias,
|
||||
3);
|
||||
|
|
@ -52,8 +51,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
|
|||
mgm0ValueCalib, 3, 3, 1);
|
||||
MatrixOperations<float>::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<float>::subtract(mgm1Value, mgmParameters->mgm1hardIronOffset, mgm1ValueNoBias,
|
||||
|
|
@ -62,8 +63,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
|
|||
mgm1ValueCalib, 3, 3, 1);
|
||||
MatrixOperations<float>::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<float>::subtract(mgm2Value, mgmParameters->mgm2hardIronOffset, mgm2ValueNoBias,
|
||||
|
|
@ -72,8 +75,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
|
|||
mgm2ValueCalib, 3, 3, 1);
|
||||
MatrixOperations<float>::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<float>::subtract(mgm3Value, mgmParameters->mgm3hardIronOffset, mgm3ValueNoBias,
|
||||
|
|
@ -82,8 +87,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
|
|||
mgm3ValueCalib, 3, 3, 1);
|
||||
MatrixOperations<float>::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<float>::subtract(mgm4Value, mgmParameters->mgm4hardIronOffset, mgm4ValueNoBias,
|
||||
|
|
@ -92,33 +99,17 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
|
|||
mgm4ValueCalib, 3, 3, 1);
|
||||
MatrixOperations<float>::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<double>::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<double>::add(gyr1ValueBody, gyr3ValueBody, gyr13ValuesSum, 3);
|
||||
VectorOperations<double>::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]) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue