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,6 +170,13 @@ 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()));
 

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);