populate the covariance fields using the noise models (#333)

Signed-off-by: Alaa El Jawad <[email protected]> Co-authored-by: Alejandro Hernández Cordero <[email protected]> Co-authored-by: Michael Carroll <[email protected]>
gazebosim · Aug 23, 2023 · 67e0dea · 67e0dea
1 parent 31b87ef
commit 67e0dea
Showing 1 changed file with 38 additions and 0 deletions.
diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc
@@ -29,6 +29,7 @@
 #include <gz/transport/Node.hh>
 
 #include "gz/sensors/ImuSensor.hh"
+#include "gz/sensors/GaussianNoiseModel.hh"
 #include "gz/sensors/Noise.hh"
 #include "gz/sensors/SensorFactory.hh"
 #include "gz/sensors/SensorTypes.hh"
@@ -255,6 +256,43 @@ bool ImuSensor::Update(const std::chrono::steady_clock::duration &_now)
   frame->set_key("frame_id");
   frame->add_value(this->FrameId());
 
+  // Populate covariance
+  for (int i = 0; i < 9; ++i) {
+    msg.mutable_linear_acceleration_covariance()->add_data(0);
+    msg.mutable_angular_velocity_covariance()->add_data(0);
+    msg.mutable_orientation_covariance()->add_data(0);
+  }
+
+  auto getCov = [&](SensorNoiseType noiseType) -> float{
+    if (this->dataPtr->noises.find(noiseType) != this->dataPtr->noises.end()) {
+      GaussianNoiseModelPtr gaussian =
+        std::dynamic_pointer_cast<GaussianNoiseModel>(
+            this->dataPtr->noises[noiseType]);
+      if (gaussian) {
+        return static_cast<float>(gaussian->StdDev() * gaussian->StdDev());
+      }
+    }
+    return 0.0f;
+  };
+
+  {
+    msg.mutable_linear_acceleration_covariance()->set_data(
+        0, getCov(ACCELEROMETER_X_NOISE_M_S_S));
+    msg.mutable_linear_acceleration_covariance()->set_data(
+        4, getCov(ACCELEROMETER_Y_NOISE_M_S_S));
+    msg.mutable_linear_acceleration_covariance()->set_data(
+        8, getCov(ACCELEROMETER_Z_NOISE_M_S_S));
+    msg.mutable_angular_velocity_covariance()->set_data(
+        0, getCov(GYROSCOPE_X_NOISE_RAD_S));
+    msg.mutable_angular_velocity_covariance()->set_data(
+        4, getCov(GYROSCOPE_Y_NOISE_RAD_S));
+    msg.mutable_angular_velocity_covariance()->set_data(
+        8, getCov(GYROSCOPE_Z_NOISE_RAD_S));
+    msg.mutable_orientation_covariance()->set_data(0, 0.0);
+    msg.mutable_orientation_covariance()->set_data(4, 0.0);
+    msg.mutable_orientation_covariance()->set_data(8, 0.0);
+  }
+
   if (this->dataPtr->orientationEnabled)
   {
     // Set the IMU orientation