From 67e0dea98bc21f942bf90ba2a16c4e4f8f488430 Mon Sep 17 00:00:00 2001
From: El Jawad Alaa <ejalaa12@gmail.com>
Date: Wed, 23 Aug 2023 14:54:32 +0200
Subject: [PATCH] populate the covariance fields using the noise models (#333)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Alaa El Jawad <ejalaa12@gmail.com>
Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
Co-authored-by: Michael Carroll <mjcarroll@intrinsic.ai>
---
 src/ImuSensor.cc | 38 ++++++++++++++++++++++++++++++++++++++
 1 file changed, 38 insertions(+)

diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc
index df53565b..b38af199 100644
--- 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