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

added bias removal for GYR
Some checks failed
EIVE/eive-obsw/pipeline/pr-develop There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-02-07 10:29:28 +01:00
parent 5d0a1858cc
commit 5f9a445aa2
2 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
mission/controller/acs/AcsParameters.h
View File

@ -770,6 +770,12 @@ class AcsParameters : public HasParametersIF {
double gyr1orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; double gyr1orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
double gyr2orientationMatrix[3][3] = {{0, 0, -1}, {0, -1, 0}, {-1, 0, 0}}; double gyr2orientationMatrix[3][3] = {{0, 0, -1}, {0, -1, 0}, {-1, 0, 0}};
double gyr3orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; double gyr3orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
double gyr0bias[3] = {0.06318149743589743, 0.4283235025641024, -0.16383500000000004};
double gyr1bias[3] = {-0.12855128205128205, 1.6737307692307695, 1.031724358974359};
double gyr2bias[3] = {0.15039212820512823, 0.7094475589743591, -0.22298363589743594};
double gyr3bias[3] = {0.0021730769230769217, -0.6655897435897435, 0.034096153846153845};
/* var = sqrt(sigma), sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is /* var = sqrt(sigma), sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is
* assumed to be equal for the same class of sensors */ * assumed to be equal for the same class of sensors */
float gyr02variance[3] = {pow(3.0e-3 * sqrt(2), 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms float gyr02variance[3] = {pow(3.0e-3 * sqrt(2), 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms

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

@ -477,6 +477,7 @@ void SensorProcessing::processGyr(
const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue}; const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value, MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value,
gyr0ValueBody, 3, 3, 1); gyr0ValueBody, 3, 3, 1);
VectorOperations<double>::subtract(gyr0ValueBody, gyrParameters->gyr0bias, gyr0ValueBody, 3);
VectorOperations<double>::mulScalar(gyr0ValueBody, M_PI / 180, gyr0ValueBody, 3); VectorOperations<double>::mulScalar(gyr0ValueBody, M_PI / 180, gyr0ValueBody, 3);
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i]; sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i];
@ -487,6 +488,7 @@ void SensorProcessing::processGyr(
const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue}; const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value, MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value,
gyr1ValueBody, 3, 3, 1); gyr1ValueBody, 3, 3, 1);
VectorOperations<double>::subtract(gyr1ValueBody, gyrParameters->gyr1bias, gyr1ValueBody, 3);
VectorOperations<double>::mulScalar(gyr1ValueBody, M_PI / 180, gyr1ValueBody, 3); VectorOperations<double>::mulScalar(gyr1ValueBody, M_PI / 180, gyr1ValueBody, 3);
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i]; sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i];
@ -497,6 +499,7 @@ void SensorProcessing::processGyr(
const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue}; const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value, MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value,
gyr2ValueBody, 3, 3, 1); gyr2ValueBody, 3, 3, 1);
VectorOperations<double>::subtract(gyr2ValueBody, gyrParameters->gyr2bias, gyr2ValueBody, 3);
VectorOperations<double>::mulScalar(gyr2ValueBody, M_PI / 180, gyr2ValueBody, 3); VectorOperations<double>::mulScalar(gyr2ValueBody, M_PI / 180, gyr2ValueBody, 3);
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i]; sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i];
@ -507,6 +510,7 @@ void SensorProcessing::processGyr(
const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue}; const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value, MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value,
gyr3ValueBody, 3, 3, 1); gyr3ValueBody, 3, 3, 1);
VectorOperations<double>::subtract(gyr3ValueBody, gyrParameters->gyr3bias, gyr3ValueBody, 3);
VectorOperations<double>::mulScalar(gyr3ValueBody, M_PI / 180, gyr3ValueBody, 3); VectorOperations<double>::mulScalar(gyr3ValueBody, M_PI / 180, gyr3ValueBody, 3);
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i]; sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i];