Add position and velocity covariance matrix for MUST sensor detection…

… data (#638)

<!-- Thanks for the contribution, this is awesome. -->

# PR Details
## Description
This PR adds position and velocity covariance matrix to Sensor Detected
Object messages created by the MUST Sensor Plugin based on configured
parameters for position variance and velocity variance
<!--- Describe your changes in detail -->

## Related Issue
[FCP-30
](https://usdot-carma.atlassian.net/browse/FCP-30)<!--- This project
only accepts pull requests related to open issues -->
<!--- If suggesting a new feature or change, please discuss it in an
issue first -->
<!--- If fixing a bug, there should be an issue describing it with steps
to reproduce -->
<!--- Please link to the issue here: -->

## Motivation and Context
For generation of SDSMs position covariance information is required to
enable fusion of sensor information
<!--- Why is this change required? What problem does it solve? -->

## How Has This Been Tested?
Unit testing and local integration testing 
<!--- Please describe in detail how you tested your changes. -->
<!--- Include details of your testing environment, and the tests you ran
to -->
<!--- see how your change affects other areas of the code, etc. -->

## Types of changes

<!--- What types of changes does your code introduce? Put an `x` in all
the boxes that apply: -->

- [x] Defect fix (non-breaking change that fixes an issue)
- [ ] New feature (non-breaking change that adds functionality)
- [ ] Breaking change (fix or feature that cause existing functionality
to change)

## Checklist:

<!--- Go over all the following points, and put an `x` in all the boxes
that apply. -->
<!--- If you're unsure about any of these, don't hesitate to ask. We're
here to help! -->

- [ ] I have added any new packages to the sonar-scanner.properties file
- [x] My change requires a change to the documentation.
- [x] I have updated the documentation accordingly.
- [x] I have read the **CONTRIBUTING** document.
[V2XHUB Contributing
Guide](https://github.com/usdot-fhwa-OPS/V2X-Hub/blob/develop/Contributing.md)
- [x] I have added tests to cover my changes.
- [x] All new and existing tests passed.

src/v2i-hub/MUSTSensorDriverPlugin/README.md

@@ -34,6 +34,13 @@ This plugin has several configuration parameters. Below these are listed out as
 > [!NOTE]
 > Both **CARMA Streets** and our vehicle automatation **CARMA Platform** rely on the PROJ4 library for projecting data between internal local maps coordinate frames and WSG84. Additional documentation on the projection string can be found in PROJ documentation (<https://proj.org/en/9.4/index.html>)(<https://proj.org/en/9.4/usage/quickstart.html>)(<https://proj.org/en/9.4/usage/transformation.html>)
 
+**DetectionPositionVariance**: Variance of the reported positon data coming from the sensor. Value is used for cooperative perception messages that enable sensor fusion like the SDMS.
+
+**DetectionVelocityVariance**: Variance of the reported velocity data coming from the sensor. Value is used for cooperative perception messages that enable sensor fusion like the SDMS.
+
+> [!NOTE]
+> Measurement variance can be calcuted from Sensor accuracy data information. For instance if a sensor position accurancy is +/- 0.5 m, assuming a normal distribution and a 95% confidence interval 0.5 m represent 2 standard deviations. Variance is equal the standard deviation squared so a position measurement accuracy of +/- 0.5 m. the variance would be 0.0625 (`(O.5/2)^2`).
+
 After setting these configuration parameters the plugin can simply be enabled.
 
 ## Design

src/v2i-hub/MUSTSensorDriverPlugin/manifest.json

@@ -37,6 +37,17 @@
             "key":"ProjectionString",
             "default":"+proj=tmerc +lat_0=0 +lon_0=0 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +geoidgrids=egm96_15.gtx +vunits=m +no_defs",
             "description":"Projection string for projecting cartesian detection data into WSG84 coordinates."
+        },
+        {
+            "key":"DetectionPositionVariance",
+            "default":"0.0625",
+            "description":"Variance of the reported positon data coming from the sensor. Value is used for cooperative perception messages that enable sensor fusion like the SDMS."
+        },
+        {
+            "key":"DetectionVelocityVariance",
+            "default":"0.0625",
+            "description":"Variance of the reported velocity data coming from the sensor. Value is used for cooperative perception messages that enable sensor fusion like the SDMS."
         }
+
     ]
 }

src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp

@@ -29,7 +29,7 @@ namespace MUSTSensorDriverPlugin {
         return detection;
     }
 
-    tmx::messages::SensorDetectedObject mustDetectionToSensorDetectedObject(const MUSTSensorDetection &detection, std::string_view sensorId, std::string_view projString) {
+    tmx::messages::SensorDetectedObject mustDetectionToSensorDetectedObject(const MUSTSensorDetection &detection, std::string_view sensorId, std::string_view projString, double positionVariance, double velocityVariance) {
         tmx::messages::SensorDetectedObject detectedObject;
         detectedObject.set_objectId(detection.trackID);
         tmx::messages::Position pos(detection.position_x, detection.position_y, 0);
@@ -38,8 +38,21 @@ namespace MUSTSensorDriverPlugin {
         detectedObject.set_timestamp(static_cast<long>(detection.timestamp*1000)); // convert decimal seconds to int milliseconds.
         detectedObject.set_velocity(headingSpeedToVelocity(detection.heading, detection.speed));
         detectedObject.set_type(detectionClassificationToSensorDetectedObjectType(detection.cl));
+        std::vector<std::vector< tmx::messages::Covariance>> positionCov(3, std::vector<tmx::messages::Covariance>(3,tmx::messages::Covariance(0.0) ));
+        // Set X and Y position variance in covariance matrix
+        positionCov[0][0] = tmx::messages::Covariance(positionVariance);
+        positionCov[1][1] = tmx::messages::Covariance(positionVariance);
+        detectedObject.set_positionCovariance(positionCov);
+
+        // Set X and Y Velocity variance in covariance matrix
+        std::vector<std::vector< tmx::messages::Covariance>> velocityCov(3, std::vector<tmx::messages::Covariance>(3,tmx::messages::Covariance(0.0) ));
+        velocityCov[0][0] = tmx::messages::Covariance(velocityVariance);
+        velocityCov[1][1] = tmx::messages::Covariance(velocityVariance);
+        detectedObject.set_velocityCovariance(velocityCov);
+
         detectedObject.set_sensorId(std::string(sensorId));
         detectedObject.set_projString(std::string(projString));
+
         return detectedObject;
     }
     DetectionClassification fromStringToDetectionClassification(const std::string &str) noexcept {

src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h

@@ -116,7 +116,7 @@ namespace MUSTSensorDriverPlugin {
      * @param projString std::string describing reference point and WGS84 projection of detection information
      * @return tmx::messages::SensorDetectedObject
      */
-    tmx::messages::SensorDetectedObject mustDetectionToSensorDetectedObject(const MUSTSensorDetection &detection, std::string_view sensorId, std::string_view projString);
+    tmx::messages::SensorDetectedObject mustDetectionToSensorDetectedObject(const MUSTSensorDetection &detection, std::string_view sensorId, std::string_view projString, double positionVariance, double velocityVariance);
 
     /**
      * @brief Function to convert MUSTSensor provided heading and speed to a velocity vector 

src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.cpp

@@ -49,6 +49,8 @@ namespace MUSTSensorDriverPlugin {
 			unsigned int port;
 			GetConfigValue<std::string>("DetectionReceiverIP", ip_address);
 			GetConfigValue<uint>("DetectionReceiverPort", port);
+			GetConfigValue<double>("DetectionPositionVariance", positionVariance);
+			GetConfigValue<double>("DetectionVelocityVariance", velocityVariance);
 			createUdpServer(ip_address, port);
 			SetStatus(keyMUSTSensorConnectionStatus, "IDLE");
 
@@ -69,7 +71,7 @@ namespace MUSTSensorDriverPlugin {
 					connected = true;
 					SetStatus(keyMUSTSensorConnectionStatus, "CONNECTED");
 				}
-				tmx::messages::SensorDetectedObject msg = mustDetectionToSensorDetectedObject(detection, sensorId, projString);
+				tmx::messages::SensorDetectedObject msg = mustDetectionToSensorDetectedObject(detection, sensorId, projString,positionVariance, velocityVariance);
 				PLOG(logDEBUG1) << "Sending Simulated SensorDetectedObject Message " << msg << std::endl;
 				this->BroadcastMessage<tmx::messages::SensorDetectedObject>(msg, _name, 0 , IvpMsgFlags_None);
 			}

src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h

@@ -48,6 +48,10 @@ namespace MUSTSensorDriverPlugin
 
             std::string projString;
 
+            double positionVariance = 0.0;
+
+            double velocityVariance = 0.0;
+
             bool connected = false;
 
             // Message receiver thread id

src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp

@@ -62,8 +62,8 @@ TEST(TestMUSTSensorDetection, mustDetectionToSensorDetectedObject ) {
     detection.timestamp = 1719506355.4;
     detection.trackID = 324;
     detection.speed = 5;
-
-    auto sensorDetectedObject = mustDetectionToSensorDetectedObject(detection, "MUSTSensor1", "PROJ String");
+    // 0.0625 variance corresponds to 0.25 std. Assuming Normal distribution and a 95% confidence interval corresponds to +/- 0.5m or m/s respectively. 
+    auto sensorDetectedObject = mustDetectionToSensorDetectedObject(detection, "MUSTSensor1", "PROJ String", 0.0625, 0.0625);
 
     EXPECT_EQ(detection.trackID, sensorDetectedObject.get_objectId());
     EXPECT_DOUBLE_EQ(detection.confidence/100.0, sensorDetectedObject.get_confidence());
@@ -75,6 +75,26 @@ TEST(TestMUSTSensorDetection, mustDetectionToSensorDetectedObject ) {
     EXPECT_EQ(1719506355400, sensorDetectedObject.get_timestamp());
     EXPECT_EQ("MUSTSensor1", sensorDetectedObject.get_sensorId());
     EXPECT_EQ("PROJ String", sensorDetectedObject.get_projString());
+    EXPECT_DOUBLE_EQ( 0.0625, sensorDetectedObject.get_positionCovariance()[0][0].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_positionCovariance()[0][1].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_positionCovariance()[0][2].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_positionCovariance()[1][0].value);
+    EXPECT_DOUBLE_EQ( 0.0625, sensorDetectedObject.get_positionCovariance()[1][1].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_positionCovariance()[1][2].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_positionCovariance()[2][0].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_positionCovariance()[2][1].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_positionCovariance()[2][2].value);
+
+    EXPECT_DOUBLE_EQ( 0.0625, sensorDetectedObject.get_velocityCovariance()[0][0].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_velocityCovariance()[0][1].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_velocityCovariance()[0][2].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_velocityCovariance()[1][0].value);
+    EXPECT_DOUBLE_EQ( 0.0625, sensorDetectedObject.get_velocityCovariance()[1][1].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_velocityCovariance()[1][2].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_velocityCovariance()[2][0].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_velocityCovariance()[2][1].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_velocityCovariance()[2][2].value);
+
 }
 
 TEST(TestMUSTSensorDetection, detectionClassificationToSensorDetectedObjectType ) {