/// \brief This function adds noise to acceleration and angular rates for
/// accelerometer and gyroscope measurement simulation.
void GazeboImuPlugin::addNoise(Eigen::Vector3d* linear_acceleration,
Eigen::Vector3d* angular_velocity,
const double dt) {
ROS_ASSERT(linear_acceleration != nullptr);
ROS_ASSERT(angular_velocity != nullptr);
ROS_ASSERT(dt > 0.0);
// Gyrosocpe
double tau_g = imu_parameters_.gyroscope_bias_correlation_time;
// Discrete-time standard deviation equivalent to an "integrating" sampler
// with integration time dt.
double sigma_g_d = 1 / sqrt(dt) * imu_parameters_.gyroscope_noise_density;
double sigma_b_g = imu_parameters_.gyroscope_random_walk;
// Compute exact covariance of the process after dt [Maybeck 4-114].
double sigma_b_g_d =
sqrt( - sigma_b_g * sigma_b_g * tau_g / 2.0 *
(exp(-2.0 * dt / tau_g) - 1.0));
// Compute state-transition.
double phi_g_d = exp(-1.0 / tau_g * dt);
// Simulate gyroscope noise processes and add them to the true angular rate.
for (int i = 0; i < 3; ++i) {
gyroscope_bias_[i] = phi_g_d * gyroscope_bias_[i] +
sigma_b_g_d * standard_normal_distribution_(random_generator_);
(*angular_velocity)[i] = (*angular_velocity)[i] +
gyroscope_bias_[i] +
sigma_g_d * standard_normal_distribution_(random_generator_) +
gyroscope_turn_on_bias_[i];
}
// Accelerometer
double tau_a = imu_parameters_.accelerometer_bias_correlation_time;
// Discrete-time standard deviation equivalent to an "integrating" sampler
// with integration time dt.
double sigma_a_d = 1 / sqrt(dt) * imu_parameters_.accelerometer_noise_density;
double sigma_b_a = imu_parameters_.accelerometer_random_walk;
// Compute exact covariance of the process after dt [Maybeck 4-114].
double sigma_b_a_d =
sqrt( - sigma_b_a * sigma_b_a * tau_a / 2.0 *
(exp(-2.0 * dt / tau_a) - 1.0));
// Compute state-transition.
double phi_a_d = exp(-1.0 / tau_a * dt);
// Simulate accelerometer noise processes and add them to the true linear
// acceleration.
for (int i = 0; i < 3; ++i) {
accelerometer_bias_[i] = phi_a_d * accelerometer_bias_[i] +
sigma_b_a_d * standard_normal_distribution_(random_generator_);
(*linear_acceleration)[i] = (*linear_acceleration)[i] +
accelerometer_bias_[i] +
sigma_a_d * standard_normal_distribution_(random_generator_) +
accelerometer_turn_on_bias_[i];
}
}
void GazeboAltimeterPlugin::OnUpdate(const common::UpdateInfo& _info)
{
// check if time to publish
common::Time current_time = world_->GetSimTime();
if((current_time - last_time_).Double() > 1.0/pub_rate_){
// pull z measurement out of Gazebo
math::Pose current_state_LFU = link_->GetWorldPose();
alt_message_.range = current_state_LFU.pos.z;
// zero measurement when messages are out of range
if(alt_message_.range > max_range_ || alt_message_.range < min_range_){
alt_message_.range = 0.0;
}
// add noise, if requested
if(alt_noise_on_){
alt_message_.range += standard_normal_distribution_(random_generator_);
}
// publish message
alt_message_.header.stamp = ros::Time::now();
if(publish_float_)
{
std_msgs::Float32 msg;
msg.data = alt_message_.range;
alt_pub_.publish(msg);
}
else
alt_pub_.publish(alt_message_);
last_time_ = current_time;
}
}
void GazeboMavlinkInterface::ImuCallback(ImuPtr& imu_message) {
math::Pose T_W_I = model_->GetWorldPose();
math::Vector3 pos_W_I = T_W_I.pos; // Use the models'world position for GPS and pressure alt.
math::Quaternion C_W_I;
C_W_I.w = imu_message->orientation().w();
C_W_I.x = imu_message->orientation().x();
C_W_I.y = imu_message->orientation().y();
C_W_I.z = imu_message->orientation().z();
math::Vector3 mag_I = C_W_I.RotateVectorReverse(mag_W_); // TODO: Add noise based on bais and variance like for imu and gyro
math::Vector3 body_vel = C_W_I.RotateVectorReverse(model_->GetWorldLinearVel());
standard_normal_distribution_ = std::normal_distribution<float>(0, 0.01f);
float mag_noise = standard_normal_distribution_(random_generator_);
if(use_mavlink_udp){
mavlink_hil_sensor_t sensor_msg;
sensor_msg.time_usec = world_->GetSimTime().nsec*1000;
sensor_msg.xacc = imu_message->linear_acceleration().x();
sensor_msg.yacc = imu_message->linear_acceleration().y();
sensor_msg.zacc = imu_message->linear_acceleration().z();
sensor_msg.xgyro = imu_message->angular_velocity().x();
sensor_msg.ygyro = imu_message->angular_velocity().y();
sensor_msg.zgyro = imu_message->angular_velocity().z();
sensor_msg.xmag = mag_I.x + mag_noise;
sensor_msg.ymag = mag_I.y + mag_noise;
sensor_msg.zmag = mag_I.z + mag_noise;
sensor_msg.abs_pressure = 0.0;
sensor_msg.diff_pressure = 0.5*1.2754*body_vel.x*body_vel.x;
sensor_msg.pressure_alt = pos_W_I.z;
sensor_msg.temperature = 0.0;
sensor_msg.fields_updated = 4095;
send_mavlink_message(MAVLINK_MSG_ID_HIL_SENSOR, &sensor_msg, 200);
} else{
hil_sensor_msg_.set_time_usec(world_->GetSimTime().nsec*1000);
hil_sensor_msg_.set_xacc(imu_message->linear_acceleration().x());
hil_sensor_msg_.set_yacc(imu_message->linear_acceleration().y());
hil_sensor_msg_.set_zacc(imu_message->linear_acceleration().z());
hil_sensor_msg_.set_xgyro(imu_message->angular_velocity().x());
hil_sensor_msg_.set_ygyro(imu_message->angular_velocity().y());
hil_sensor_msg_.set_zgyro(imu_message->angular_velocity().z());
hil_sensor_msg_.set_xmag(mag_I.x);
hil_sensor_msg_.set_ymag(mag_I.y);
hil_sensor_msg_.set_zmag(mag_I.z);
hil_sensor_msg_.set_abs_pressure(0.0);
hil_sensor_msg_.set_diff_pressure(0.5*1.2754*body_vel.x*body_vel.x);
hil_sensor_msg_.set_pressure_alt(pos_W_I.z);
hil_sensor_msg_.set_temperature(0.0);
hil_sensor_msg_.set_fields_updated(4095); // 0b1111111111111 (All updated since new data with new noise added always)
hil_sensor_pub_->Publish(hil_sensor_msg_);
}
}
void GazeboMavlinkInterface::ImuCallback(ImuPtr& imu_message) {
math::Pose T_W_I = model_->GetWorldPose();
math::Vector3 pos_W_I = T_W_I.pos; // Use the models'world position for GPS and pressure alt.
math::Quaternion C_W_I;
C_W_I.w = imu_message->orientation().w();
C_W_I.x = imu_message->orientation().x();
C_W_I.y = imu_message->orientation().y();
C_W_I.z = imu_message->orientation().z();
// gzerr << "got imu: " << C_W_I << "\n";
float declination = get_mag_declination(lat_rad, lon_rad);
math::Quaternion C_D_I(0.0, 0.0, declination);
math::Vector3 mag_decl = C_D_I.RotateVectorReverse(mag_W_);
// TODO replace mag_W_ in the line below with mag_decl
math::Vector3 mag_I = C_W_I.RotateVectorReverse(mag_decl); // TODO: Add noise based on bais and variance like for imu and gyro
math::Vector3 body_vel = C_W_I.RotateVectorReverse(model_->GetWorldLinearVel());
standard_normal_distribution_ = std::normal_distribution<float>(0, 0.01f);
float mag_noise = standard_normal_distribution_(random_generator_);
mavlink_hil_sensor_t sensor_msg;
sensor_msg.time_usec = world_->GetSimTime().nsec*1000;
sensor_msg.xacc = imu_message->linear_acceleration().x();
sensor_msg.yacc = imu_message->linear_acceleration().y();
sensor_msg.zacc = imu_message->linear_acceleration().z();
sensor_msg.xgyro = imu_message->angular_velocity().x();
sensor_msg.ygyro = imu_message->angular_velocity().y();
sensor_msg.zgyro = imu_message->angular_velocity().z();
sensor_msg.xmag = mag_I.x + mag_noise;
sensor_msg.ymag = mag_I.y + mag_noise;
sensor_msg.zmag = mag_I.z + mag_noise;
sensor_msg.abs_pressure = 0.0;
sensor_msg.diff_pressure = 0.5*1.2754*(body_vel.z + body_vel.x)*(body_vel.z + body_vel.x) / 100;
sensor_msg.pressure_alt = pos_W_I.z;
sensor_msg.temperature = 0.0;
sensor_msg.fields_updated = 4095;
//gyro needed for optical flow message
optflow_xgyro = imu_message->angular_velocity().x();
optflow_ygyro = imu_message->angular_velocity().y();
optflow_zgyro = imu_message->angular_velocity().z();
send_mavlink_message(MAVLINK_MSG_ID_HIL_SENSOR, &sensor_msg, 200);
}
void GazeboMavlinkInterface::ImuCallback(const sensor_msgs::ImuConstPtr& imu_message) {
mavlink_message_t mmsg;
math::Pose T_W_I = model_->GetWorldPose();
math::Vector3 pos_W_I = T_W_I.pos; // Use the models'world position for GPS and pressure alt.
math::Quaternion C_W_I;
C_W_I.w = imu_message->orientation.w;
C_W_I.x = imu_message->orientation.x;
C_W_I.y = imu_message->orientation.y;
C_W_I.z = imu_message->orientation.z;
math::Vector3 mag_I = C_W_I.RotateVectorReverse(mag_W_); // TODO: Add noise based on bais and variance like for imu and gyro
math::Vector3 body_vel = C_W_I.RotateVectorReverse(model_->GetWorldLinearVel());
standard_normal_distribution_ = std::normal_distribution<float>(0, 0.01f);
float mag_noise = standard_normal_distribution_(random_generator_);
hil_sensor_msg_.time_usec = imu_message->header.stamp.nsec*1000;
hil_sensor_msg_.xacc = imu_message->linear_acceleration.x;
hil_sensor_msg_.yacc = imu_message->linear_acceleration.y;
hil_sensor_msg_.zacc = imu_message->linear_acceleration.z;
hil_sensor_msg_.xgyro = imu_message->angular_velocity.x;
hil_sensor_msg_.ygyro = imu_message->angular_velocity.y;
hil_sensor_msg_.zgyro = imu_message->angular_velocity.z;
hil_sensor_msg_.xmag = mag_I.x + mag_noise;
hil_sensor_msg_.ymag = mag_I.y + mag_noise;;
hil_sensor_msg_.zmag = mag_I.z + mag_noise;;
hil_sensor_msg_.abs_pressure = 0.0;
hil_sensor_msg_.diff_pressure = 0.5*1.2754*body_vel.x*body_vel.x;
hil_sensor_msg_.pressure_alt = pos_W_I.z;
hil_sensor_msg_.temperature = 0.0;
hil_sensor_msg_.fields_updated = 4095; // 0b1111111111111 (All updated since new data with new noise added always)
mavlink_hil_sensor_t* hil_msg = &hil_sensor_msg_;
mavlink_msg_hil_sensor_encode(1, 240, &mmsg, hil_msg);
mavlink_message_t* msg = &mmsg;
mavros_msgs::MavlinkPtr rmsg = boost::make_shared<mavros_msgs::Mavlink>();
rmsg->header.stamp = ros::Time::now();
mavros_msgs::mavlink::convert(*msg, *rmsg);
hil_sensor_pub_.publish(rmsg);
}
// This gets called by the world update start event.
void GazeboAirspeedPlugin::OnUpdate(const common::UpdateInfo& _info) {
// Calculate Airspeed
math::Vector3 C_linear_velocity_W_C = link_->GetRelativeLinearVel();
double u = C_linear_velocity_W_C.x;
double v = -C_linear_velocity_W_C.y;
double w = -C_linear_velocity_W_C.z;
double ur = u - wind_.N;
double vr = v - wind_.E;
double wr = w - wind_.D;
// double Va = sqrt(pow(ur,2.0) + pow(vr,2.0) + pow(wr,2.0));
double Va = sqrt(pow(u,2.0) + pow(v,2.0) + pow(w,2.0));
// Invert Airpseed to get sensor measurement
double y = rho_*Va*Va/2.0; // Page 130 in the UAV Book
y += pressure_bias_ + pressure_noise_sigma_*standard_normal_distribution_(random_generator_);
y = (y>max_pressure_)?max_pressure_:y;
y = (y<min_pressure_)?min_pressure_:y;
airspeed_message_.fluid_pressure = y;
airspeed_pub_.publish(airspeed_message_);
}
void GazeboImuPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
// Store the pointer to the model
model_ = _model;
world_ = model_->GetWorld();
// default params
namespace_.clear();
if (_sdf->HasElement("robotNamespace"))
namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
else
gzerr << "[gazebo_imu_plugin] Please specify a robotNamespace.\n";
node_handle_ = new ros::NodeHandle(namespace_);
if (_sdf->HasElement("linkName"))
link_name_ = _sdf->GetElement("linkName")->Get<std::string>();
else
gzerr << "[gazebo_imu_plugin] Please specify a linkName.\n";
// Get the pointer to the link
link_ = model_->GetLink(link_name_);
if (link_ == NULL)
gzthrow("[gazebo_imu_plugin] Couldn't find specified link \"" << link_name_ << "\".");
frame_id_ = link_name_;
getSdfParam<std::string>(_sdf, "imuTopic", imu_topic_, kDefaultImuTopic);
getSdfParam<double>(_sdf, "gyroscopeNoiseDensity",
imu_parameters_.gyroscope_noise_density,
imu_parameters_.gyroscope_noise_density);
getSdfParam<double>(_sdf, "gyroscopeBiasRandomWalk",
imu_parameters_.gyroscope_random_walk,
imu_parameters_.gyroscope_random_walk);
getSdfParam<double>(_sdf, "gyroscopeBiasCorrelationTime",
imu_parameters_.gyroscope_bias_correlation_time,
imu_parameters_.gyroscope_bias_correlation_time);
assert(imu_parameters_.gyroscope_bias_correlation_time > 0.0);
getSdfParam<double>(_sdf, "gyroscopeTurnOnBiasSigma",
imu_parameters_.gyroscope_turn_on_bias_sigma,
imu_parameters_.gyroscope_turn_on_bias_sigma);
getSdfParam<double>(_sdf, "accelerometerNoiseDensity",
imu_parameters_.accelerometer_noise_density,
imu_parameters_.accelerometer_noise_density);
getSdfParam<double>(_sdf, "accelerometerRandomWalk",
imu_parameters_.accelerometer_random_walk,
imu_parameters_.accelerometer_random_walk);
getSdfParam<double>(_sdf, "accelerometerBiasCorrelationTime",
imu_parameters_.accelerometer_bias_correlation_time,
imu_parameters_.accelerometer_bias_correlation_time);
assert(imu_parameters_.accelerometer_bias_correlation_time > 0.0);
getSdfParam<double>(_sdf, "accelerometerTurnOnBiasSigma",
imu_parameters_.accelerometer_turn_on_bias_sigma,
imu_parameters_.accelerometer_turn_on_bias_sigma);
last_time_ = world_->GetSimTime();
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->updateConnection_ =
event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboImuPlugin::OnUpdate, this, _1));
imu_pub_ = node_handle_->advertise<sensor_msgs::Imu>(imu_topic_, 10);
// Fill imu message.
imu_message_.header.frame_id = frame_id_;
// We assume uncorrelated noise on the 3 channels -> only set diagonal
// elements. Only the broadband noise component is considered, specified as a
// continuous-time density (two-sided spectrum); not the true covariance of
// the measurements.
// Angular velocity measurement covariance.
imu_message_.angular_velocity_covariance[0] =
imu_parameters_.gyroscope_noise_density *
imu_parameters_.gyroscope_noise_density;
imu_message_.angular_velocity_covariance[4] =
imu_parameters_.gyroscope_noise_density *
imu_parameters_.gyroscope_noise_density;
imu_message_.angular_velocity_covariance[8] =
imu_parameters_.gyroscope_noise_density *
imu_parameters_.gyroscope_noise_density;
// Linear acceleration measurement covariance.
imu_message_.linear_acceleration_covariance[0] =
imu_parameters_.accelerometer_noise_density *
imu_parameters_.accelerometer_noise_density;
imu_message_.linear_acceleration_covariance[4] =
imu_parameters_.accelerometer_noise_density *
imu_parameters_.accelerometer_noise_density;
imu_message_.linear_acceleration_covariance[8] =
imu_parameters_.accelerometer_noise_density *
imu_parameters_.accelerometer_noise_density;
// Orientation estimate covariance (no estimate provided).
imu_message_.orientation_covariance[0] = -1.0;
gravity_W_ = world_->GetPhysicsEngine()->GetGravity();
imu_parameters_.gravity_magnitude = gravity_W_.GetLength();
standard_normal_distribution_ = std::normal_distribution<double>(0.0, 1.0);
double sigma_bon_g = imu_parameters_.gyroscope_turn_on_bias_sigma;
double sigma_bon_a = imu_parameters_.accelerometer_turn_on_bias_sigma;
for (int i = 0; i < 3; ++i) {
gyroscope_turn_on_bias_[i] =
sigma_bon_g * standard_normal_distribution_(random_generator_);
accelerometer_turn_on_bias_[i] =
sigma_bon_a * standard_normal_distribution_(random_generator_);
}
// TODO(nikolicj) incorporate steady-state covariance of bias process
gyroscope_bias_.setZero();
accelerometer_bias_.setZero();
}
