SITL: Add gausian noise and rate to VICON sim

Author: rishabsingh3003

libraries/SITL/SIM_Vicon.cpp

@@ -113,8 +113,13 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
         return;
     }
 
-    if (now_us - last_observation_usec < 20000) {
-        // create observations at 20ms intervals (matches EKF max rate)
+    if (_sitl->vicon_rate_hz == 0) {
+        return;
+    }
+    const uint64_t vicon_interval_us = 1000000UL / _sitl->vicon_rate_hz;  // Interval in microseconds based on rate
+    if (now_us - last_observation_usec < vicon_interval_us) {
+        // create observations at rate specified by vicon_rate_hz
+        // by default runs at 50Hz
         return;
     }
 
@@ -136,6 +141,12 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
 
     // add earth frame sensor offset and glitch to position
     Vector3d pos_corrected = position + (pos_offset_ef + _sitl->vicon_glitch.get()).todouble();
+    // add some gaussian noise to the position
+    pos_corrected += Vector3d(
+        Aircraft::rand_normal(0, _sitl->vicon_pos_stddev),
+        Aircraft::rand_normal(0, _sitl->vicon_pos_stddev),
+        Aircraft::rand_normal(0, _sitl->vicon_pos_stddev)
+    );
 
     // calculate a velocity offset due to the antenna position offset and body rotation rate
     // note: % operator is overloaded for cross product
@@ -159,17 +170,33 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
         vel_corrected = vicon_yaw_rot.tofloat() * vel_corrected;
     }
 
+    // add some gaussian noise to the velocity
+    vel_corrected += Vector3f(
+        Aircraft::rand_normal(0, _sitl->vicon_vel_stddev),
+        Aircraft::rand_normal(0, _sitl->vicon_vel_stddev),
+        Aircraft::rand_normal(0, _sitl->vicon_vel_stddev)
+    );
+
     // add yaw error reported to vehicle
     yaw = wrap_PI(yaw + radians(_sitl->vicon_yaw_error.get()));
 
     // 25ms to 124ms delay before sending
     uint32_t delay_ms = 25 + unsigned(random()) % 100;
     uint64_t time_send_us = now_us + delay_ms * 1000UL;
 
+
+    float pose_cov[21];
+    memset(pose_cov, 0, sizeof(pose_cov));
+    // Set variances (diagonal elements), assume no cross-correlation. TODO: figure out attitude variances
+    const float pos_variance = _sitl->vicon_pos_stddev*_sitl->vicon_pos_stddev;
+    pose_cov[0]  = pos_variance;   // x
+    pose_cov[6]  = pos_variance;   // y
+    pose_cov[11] = pos_variance;   // z
+
     // send vision position estimate message
     uint8_t msg_buf_index;
     if (should_send(ViconTypeMask::VISION_POSITION_ESTIMATE) && get_free_msg_buf_index(msg_buf_index)) {
-        const mavlink_vision_position_estimate_t vision_position_estimate{
+        mavlink_vision_position_estimate_t vision_position_estimate{
         usec: now_us + time_offset_us,
         x: float(pos_corrected.x),
         y: float(pos_corrected.y),
@@ -178,6 +205,8 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
         pitch: pitch,
         yaw: yaw
         };
+        memcpy(vision_position_estimate.covariance, pose_cov, sizeof(pose_cov));
+
         mavlink_msg_vision_position_estimate_encode_status(
             system_id,
             component_id,
@@ -210,12 +239,21 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
 
     // send vision speed estimate
     if (should_send(ViconTypeMask::VISION_SPEED_ESTIMATE) && get_free_msg_buf_index(msg_buf_index)) {
-        const mavlink_vision_speed_estimate_t vicon_speed_estimate{
+        float cov[9];
+        memset(cov, 0, sizeof(cov));
+        // Set variances (diagonal elements), assume no cross-correlation
+        const float vel_variance = _sitl->vicon_vel_stddev*_sitl->vicon_vel_stddev;
+        cov[0] = vel_variance;   // x
+        cov[4] = vel_variance;   // y
+        cov[8] = vel_variance;   // z
+        mavlink_vision_speed_estimate_t vicon_speed_estimate{
         usec: now_us + time_offset_us,
         x: vel_corrected.x,
         y: vel_corrected.y,
         z: vel_corrected.z
         };
+        memcpy(vicon_speed_estimate.covariance, cov, sizeof(cov));
+
         mavlink_msg_vision_speed_estimate_encode_status(
             system_id,
             component_id,
@@ -230,7 +268,15 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
     // send ODOMETRY message
     if (should_send(ViconTypeMask::ODOMETRY) && get_free_msg_buf_index(msg_buf_index)) {
         const Vector3f vel_corrected_frd = attitude.inverse() * vel_corrected;
-        const mavlink_odometry_t odometry{
+        float vel_cov[21];
+        memset(vel_cov, 0, sizeof(vel_cov));
+        // Set variances (diagonal elements), assume no cross-correlation. TODO: figure out angular velocity variances
+        const float vel_variance = _sitl->vicon_vel_stddev*_sitl->vicon_vel_stddev;
+        vel_cov[0] = vel_variance;   // x
+        vel_cov[6] = vel_variance;   // y
+        vel_cov[11] = vel_variance;   // z
+
+        mavlink_odometry_t odometry{
         time_usec: now_us + time_offset_us,
         x: float(pos_corrected.x),
         y: float(pos_corrected.y),
@@ -250,6 +296,9 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
         estimator_type: MAV_ESTIMATOR_TYPE_VIO,
         quality: 50, // quality hardcoded to 50%
         };
+        memcpy(odometry.pose_covariance, pose_cov, sizeof(pose_cov));
+        memcpy(odometry.velocity_covariance, vel_cov, sizeof(vel_cov));
+
         mavlink_msg_odometry_encode_status(
             system_id,
             component_id,

libraries/SITL/SITL.cpp

@@ -724,6 +724,27 @@ const AP_Param::GroupInfo SIM::var_info3[] = {
     AP_SUBGROUPINFO(volz_sim, "VOLZ_", 55, SIM, Volz),
 #endif
 
+    // @Param: VICON_P_SD
+    // @DisplayName: SITL vicon position standard deviation for gaussian noise
+    // @Description: SITL vicon position standard deviation for gaussian noise
+    // @Units: m
+    // @User: Advanced
+    AP_GROUPINFO("VICON_P_SD",  56, SIM,  vicon_pos_stddev, 0.0f),
+
+    // @Param: VICON_V_SD
+    // @DisplayName: SITL vicon velocity standard deviation for gaussian noise
+    // @Description: SITL vicon velocity standard deviation for gaussian noise
+    // @Units: m/s
+    // @User: Advanced
+    AP_GROUPINFO("VICON_V_SD",  57, SIM,  vicon_vel_stddev, 0.0f),
+
+    // @Param: VICON_RATE
+    // @DisplayName: SITL vicon rate
+    // @Description: SITL vicon rate
+    // @Units: Hz
+    // @User: Advanced
+    AP_GROUPINFO("VICON_RATE",  58, SIM,  vicon_rate_hz, 50),
+
 #ifdef SFML_JOYSTICK
     AP_SUBGROUPEXTENSION("",      63, SIM,  var_sfml_joystick),
 #endif // SFML_JOYSTICK

libraries/SITL/SITL.h

@@ -538,11 +538,14 @@ class SIM {
 
     // vicon parameters
     AP_Vector3f vicon_glitch;   // glitch in meters in vicon's local NED frame
+    AP_Float vicon_pos_stddev;       // noise in meters in vicon's local NED frame
+    AP_Float vicon_vel_stddev;       // noise in m/s in vicon's local NED frame
     AP_Int8 vicon_fail;         // trigger vicon failure
     AP_Int16 vicon_yaw;         // vicon local yaw in degrees
     AP_Int16 vicon_yaw_error;   // vicon yaw error in degrees (added to reported yaw sent to vehicle)
     AP_Int8 vicon_type_mask;    // vicon message type mask (bit0:vision position estimate, bit1:vision speed estimate, bit2:vicon position estimate)
     AP_Vector3f vicon_vel_glitch;   // velocity glitch in m/s in vicon's local frame
+    AP_Int16 vicon_rate_hz;     // vicon data rate in Hz
 
     // get the rangefinder reading for the desired instance, returns -1 for no data
     float get_rangefinder(uint8_t instance);