Skip to content
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

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

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
+78 −5
Open

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
59 changes: 54 additions & 5 deletions libraries/SITL/SIM_Vicon.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand All @@ -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
Expand All @@ -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),
Expand All @@ -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,
Expand Down Expand Up @@ -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,
Expand All @@ -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),
Expand All @@ -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,
Expand Down
21 changes: 21 additions & 0 deletions libraries/SITL/SITL.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -724,6 +724,27 @@ const AP_Param::GroupInfo SIM::var_info3[] = {
AP_SUBGROUPINFO(volz_sim, "VOLZ_", 55, SIM, Volz),
#endif

// @Param: VICON_P_SD
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

should be a subgroup, ideally move the params into SIM_Vicon.cpp

// @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
Expand Down
3 changes: 3 additions & 0 deletions libraries/SITL/SITL.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
Loading