From 5f9a445aa22f87a9f4ffd80d8df5f3e7b6a189cd Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Tue, 7 Feb 2023 10:29:28 +0100
Subject: [PATCH] added bias removal for GYR

---
 mission/controller/acs/AcsParameters.h      | 6 ++++++
 mission/controller/acs/SensorProcessing.cpp | 4 ++++
 2 files changed, 10 insertions(+)

diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h
index 88802a46..1ffb33c7 100644
--- a/mission/controller/acs/AcsParameters.h
+++ b/mission/controller/acs/AcsParameters.h
@@ -770,6 +770,12 @@ class AcsParameters : public HasParametersIF {
     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 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
      * 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
diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp
index bfb38afc..0496e7e9 100644
--- a/mission/controller/acs/SensorProcessing.cpp
+++ b/mission/controller/acs/SensorProcessing.cpp
@@ -477,6 +477,7 @@ void SensorProcessing::processGyr(
     const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue};
     MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value,
                                        gyr0ValueBody, 3, 3, 1);
+    VectorOperations<double>::subtract(gyr0ValueBody, gyrParameters->gyr0bias, gyr0ValueBody, 3);
     VectorOperations<double>::mulScalar(gyr0ValueBody, M_PI / 180, gyr0ValueBody, 3);
     for (uint8_t i = 0; i < 3; i++) {
       sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i];
@@ -487,6 +488,7 @@ void SensorProcessing::processGyr(
     const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue};
     MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value,
                                        gyr1ValueBody, 3, 3, 1);
+    VectorOperations<double>::subtract(gyr1ValueBody, gyrParameters->gyr1bias, gyr1ValueBody, 3);
     VectorOperations<double>::mulScalar(gyr1ValueBody, M_PI / 180, gyr1ValueBody, 3);
     for (uint8_t i = 0; i < 3; i++) {
       sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i];
@@ -497,6 +499,7 @@ void SensorProcessing::processGyr(
     const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue};
     MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value,
                                        gyr2ValueBody, 3, 3, 1);
+    VectorOperations<double>::subtract(gyr2ValueBody, gyrParameters->gyr2bias, gyr2ValueBody, 3);
     VectorOperations<double>::mulScalar(gyr2ValueBody, M_PI / 180, gyr2ValueBody, 3);
     for (uint8_t i = 0; i < 3; i++) {
       sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i];
@@ -507,6 +510,7 @@ void SensorProcessing::processGyr(
     const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue};
     MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value,
                                        gyr3ValueBody, 3, 3, 1);
+    VectorOperations<double>::subtract(gyr3ValueBody, gyrParameters->gyr3bias, gyr3ValueBody, 3);
     VectorOperations<double>::mulScalar(gyr3ValueBody, M_PI / 180, gyr3ValueBody, 3);
     for (uint8_t i = 0; i < 3; i++) {
       sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i];