Skip to content

SITL: Add gausian noise and rate limit to VICON sim #29524

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
Apr 14, 2025
Merged

SITL: Add gausian noise and rate limit to VICON sim #29524

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
186 changes: 175 additions & 11 deletions libraries/SITL/SIM_Vicon.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,121 @@ extern const AP_HAL::HAL& hal;

using namespace SITL;

const AP_Param::GroupInfo SIM::ViconParms::var_info[] = {
// @Param: POS_X
// @DisplayName: SITL vicon position on vehicle in Forward direction
// @Description: SITL vicon position on vehicle in Forward direction
// @Units: m
// @Range: 0 10
// @User: Advanced

// @Param: POS_Y
// @DisplayName: SITL vicon position on vehicle in Right direction
// @Description: SITL vicon position on vehicle in Right direction
// @Units: m
// @Range: 0 10
// @User: Advanced

// @Param: POS_Z
// @DisplayName: SITL vicon position on vehicle in Down direction
// @Description: SITL vicon position on vehicle in Down direction
// @Units: m
// @Range: 0 10
// @User: Advanced
AP_GROUPINFO("POS", 1, ViconParms, pos_offset, 0),

// @Param: GLIT_X
// @DisplayName: SITL vicon position glitch North
// @Description: SITL vicon position glitch North
// @Units: m
// @User: Advanced

// @Param: GLIT_Y
// @DisplayName: SITL vicon position glitch East
// @Description: SITL vicon position glitch East
// @Units: m
// @User: Advanced

// @Param: GLIT_Z
// @DisplayName: SITL vicon position glitch Down
// @Description: SITL vicon position glitch Down
// @Units: m
// @User: Advanced
AP_GROUPINFO("GLIT", 2, ViconParms, glitch, 0),

// @Param: FAIL
// @DisplayName: SITL vicon failure
// @Description: SITL vicon failure
// @Values: 0:Vicon Healthy, 1:Vicon Failed
// @User: Advanced
AP_GROUPINFO("FAIL", 3, ViconParms, fail, 0),

// @Param: YAW
// @DisplayName: SITL vicon yaw angle in earth frame
// @Description: SITL vicon yaw angle in earth frame
// @Units: deg
// @Range: 0 360
// @User: Advanced
AP_GROUPINFO("YAW", 4, ViconParms, yaw, 0),

// @Param: YAWERR
// @DisplayName: SITL vicon yaw error
// @Description: SITL vicon yaw added to reported yaw sent to vehicle
// @Units: deg
// @Range: -180 180
// @User: Advanced
AP_GROUPINFO("YAWERR", 5, ViconParms, yaw_error, 0),

// @Param: TMASK
// @DisplayName: SITL vicon type mask
// @Description: SITL vicon messages sent
// @Bitmask: 0:VISION_POSITION_ESTIMATE, 1:VISION_SPEED_ESTIMATE, 2:VICON_POSITION_ESTIMATE, 3:VISION_POSITION_DELTA, 4:ODOMETRY
// @User: Advanced
AP_GROUPINFO("TMASK", 6, ViconParms, type_mask, 3),

// @Param: VGLI_X
// @DisplayName: SITL vicon velocity glitch North
// @Description: SITL vicon velocity glitch North
// @Units: m/s
// @User: Advanced

// @Param: VGLI_Y
// @DisplayName: SITL vicon velocity glitch East
// @Description: SITL vicon velocity glitch East
// @Units: m/s
// @User: Advanced

// @Param: VGLI_Z
// @DisplayName: SITL vicon velocity glitch Down
// @Description: SITL vicon velocity glitch Down
// @Units: m/s
// @User: Advanced
AP_GROUPINFO("VGLI", 7, ViconParms, vel_glitch, 0),

// @Param: 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("P_SD", 8, ViconParms, pos_stddev, 0.0f),

// @Param: 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("V_SD", 9, ViconParms, vel_stddev, 0.0f),

// @Param: RATE
// @DisplayName: SITL vicon rate
// @Description: SITL vicon rate
// @Units: Hz
// @User: Advanced
AP_GROUPINFO("RATE", 10, ViconParms, rate_hz, 50),

AP_GROUPEND
};

Vicon::Vicon() :
SerialDevice::SerialDevice()
{
Expand Down Expand Up @@ -113,13 +228,18 @@ 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;
}

// failure simulation
if (_sitl->vicon_fail.get() != 0) {
if (_sitl->vicon.fail.get() != 0) {
return;
}

Expand All @@ -129,13 +249,19 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
attitude.to_euler(roll, pitch, yaw);

// calculate sensor offset in earth frame
const Vector3f& pos_offset = _sitl->vicon_pos_offset.get();
const Vector3f& pos_offset = _sitl->vicon.pos_offset.get();
Matrix3f rot;
rot.from_euler(radians(_sitl->state.rollDeg), radians(_sitl->state.pitchDeg), radians(_sitl->state.yawDeg));
Vector3f pos_offset_ef = rot * pos_offset;

// add earth frame sensor offset and glitch to position
Vector3d pos_corrected = position + (pos_offset_ef + _sitl->vicon_glitch.get()).todouble();
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
Expand All @@ -146,10 +272,10 @@ void Vicon::update_vicon_position_estimate(const Location &loc,

// rotate the velocity offset into earth frame and add to the c.g. velocity
Vector3f vel_rel_offset_ef = rot * vel_rel_offset_bf;
Vector3f vel_corrected = velocity + vel_rel_offset_ef + _sitl->vicon_vel_glitch.get();
Vector3f vel_corrected = velocity + vel_rel_offset_ef + _sitl->vicon.vel_glitch.get();

// adjust yaw, position and velocity to account for vicon's yaw
const int16_t vicon_yaw_deg = _sitl->vicon_yaw.get();
const int16_t vicon_yaw_deg = _sitl->vicon.yaw.get();
if (vicon_yaw_deg != 0) {
const float vicon_yaw_rad = radians(vicon_yaw_deg);
yaw = wrap_PI(yaw - vicon_yaw_rad);
Expand All @@ -159,17 +285,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()));
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),
Expand All @@ -178,6 +320,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,
Expand Down Expand Up @@ -210,12 +354,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,
Expand All @@ -230,7 +383,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),
Expand All @@ -250,6 +411,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,
Expand Down
2 changes: 1 addition & 1 deletion libraries/SITL/SIM_Vicon.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ class Vicon : public SerialDevice {
};

// return true if the given message type should be sent
bool should_send(ViconTypeMask type_mask) const { return (((uint8_t)type_mask & _sitl->vicon_type_mask.get()) > 0); }
bool should_send(ViconTypeMask type_mask) const { return (((uint8_t)type_mask & _sitl->vicon.type_mask.get()) > 0); }

// get unused index in msg_buf
bool get_free_msg_buf_index(uint8_t &index);
Expand Down
94 changes: 4 additions & 90 deletions libraries/SITL/SITL.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -533,99 +533,9 @@ const AP_Param::GroupInfo SIM::var_info3[] = {
// @Description: Scenario for thermalling simulation, for soaring
AP_GROUPINFO("THML_SCENARI", 12, SIM, thermal_scenario, 0),

// @Param: VICON_POS_X
// @DisplayName: SITL vicon position on vehicle in Forward direction
// @Description: SITL vicon position on vehicle in Forward direction
// @Units: m
// @Range: 0 10
// @User: Advanced

// @Param: VICON_POS_Y
// @DisplayName: SITL vicon position on vehicle in Right direction
// @Description: SITL vicon position on vehicle in Right direction
// @Units: m
// @Range: 0 10
// @User: Advanced

// @Param: VICON_POS_Z
// @DisplayName: SITL vicon position on vehicle in Down direction
// @Description: SITL vicon position on vehicle in Down direction
// @Units: m
// @Range: 0 10
// @User: Advanced
AP_GROUPINFO("VICON_POS", 14, SIM, vicon_pos_offset, 0),

// Buyoancy for submarines
AP_GROUPINFO_FRAME("BUOYANCY", 15, SIM, buoyancy, 1, AP_PARAM_FRAME_SUB),

// @Param: VICON_GLIT_X
// @DisplayName: SITL vicon position glitch North
// @Description: SITL vicon position glitch North
// @Units: m
// @User: Advanced

// @Param: VICON_GLIT_Y
// @DisplayName: SITL vicon position glitch East
// @Description: SITL vicon position glitch East
// @Units: m
// @User: Advanced

// @Param: VICON_GLIT_Z
// @DisplayName: SITL vicon position glitch Down
// @Description: SITL vicon position glitch Down
// @Units: m
// @User: Advanced
AP_GROUPINFO("VICON_GLIT", 16, SIM, vicon_glitch, 0),

// @Param: VICON_FAIL
// @DisplayName: SITL vicon failure
// @Description: SITL vicon failure
// @Values: 0:Vicon Healthy, 1:Vicon Failed
// @User: Advanced
AP_GROUPINFO("VICON_FAIL", 17, SIM, vicon_fail, 0),

// @Param: VICON_YAW
// @DisplayName: SITL vicon yaw angle in earth frame
// @Description: SITL vicon yaw angle in earth frame
// @Units: deg
// @Range: 0 360
// @User: Advanced
AP_GROUPINFO("VICON_YAW", 18, SIM, vicon_yaw, 0),

// @Param: VICON_YAWERR
// @DisplayName: SITL vicon yaw error
// @Description: SITL vicon yaw added to reported yaw sent to vehicle
// @Units: deg
// @Range: -180 180
// @User: Advanced
AP_GROUPINFO("VICON_YAWERR", 19, SIM, vicon_yaw_error, 0),

// @Param: VICON_TMASK
// @DisplayName: SITL vicon type mask
// @Description: SITL vicon messages sent
// @Bitmask: 0:VISION_POSITION_ESTIMATE, 1:VISION_SPEED_ESTIMATE, 2:VICON_POSITION_ESTIMATE, 3:VISION_POSITION_DELTA, 4:ODOMETRY
// @User: Advanced
AP_GROUPINFO("VICON_TMASK", 20, SIM, vicon_type_mask, 3),

// @Param: VICON_VGLI_X
// @DisplayName: SITL vicon velocity glitch North
// @Description: SITL vicon velocity glitch North
// @Units: m/s
// @User: Advanced

// @Param: VICON_VGLI_Y
// @DisplayName: SITL vicon velocity glitch East
// @Description: SITL vicon velocity glitch East
// @Units: m/s
// @User: Advanced

// @Param: VICON_VGLI_Z
// @DisplayName: SITL vicon velocity glitch Down
// @Description: SITL vicon velocity glitch Down
// @Units: m/s
// @User: Advanced
AP_GROUPINFO("VICON_VGLI", 21, SIM, vicon_vel_glitch, 0),

// @Param: RATE_HZ
// @DisplayName: Loop rate
// @Description: SITL Loop rate
Expand Down Expand Up @@ -733,6 +643,10 @@ const AP_Param::GroupInfo SIM::var_info3[] = {
AP_SUBGROUPINFO(volz_sim, "VOLZ_", 55, SIM, Volz),
#endif

// @Group: VICON_
// @Path: ./SIM_Vicon.cpp
AP_SUBGROUPINFO(vicon, "VICON_", 56, SIM, ViconParms),

#ifdef SFML_JOYSTICK
AP_SUBGROUPEXTENSION("", 63, SIM, var_sfml_joystick),
#endif // SFML_JOYSTICK
Expand Down
Loading
Loading