Eliminating all sensor noise and bias

heinzketchupp · November 5, 2019, 11:49am

Hello everyone!

I’m currently making a drone trajectory tracking simulator (HITL with px4 for iris model) that doesn’t have any sensor noise or bias ( IMU, GPS, magnetometer, etc…), i.e. the sensors are perfect.

I wonder if there is any way I can do that. I’m struggling with this problem.

I tried to reduce and set the sensor parameters to 0 in the " iris.sdf " file and it didn’t work.

My specs:

PX4 v1.9.2
QGC v3.5.5

Thanks for helping!

JulianOes · November 5, 2019, 2:06pm

Have a look at these places:

github.com

PX4/sitl_gazebo/blob/169d48217df89922e9fae72ef34fa46ce2e209dd/src/gazebo_mavlink_interface.cpp#L776-L787


// Let's use a rough guess of 0.05 hPa as the standard devitiation which roughly yields
// about +/- 1 m/s noise.
const float diff_pressure_stddev = 0.05f;
const float diff_pressure_noise = standard_normal_distribution_(random_generator_) * diff_pressure_stddev;


// calculate differential pressure in hPa
// if vehicle is a tailsitter the airspeed axis is different (z points from nose to tail)
if (vehicle_is_tailsitter_) {
  sensor_msg.diff_pressure = 0.005f*rho*vel_b.Z()*vel_b.Z() + diff_pressure_noise;
} else {
  sensor_msg.diff_pressure = 0.005f*rho*vel_b.X()*vel_b.X() + diff_pressure_noise;
}

github.com

PX4/sitl_gazebo/blob/169d48217df89922e9fae72ef34fa46ce2e209dd/src/gazebo_imu_plugin.cpp#L189-L241


void GazeboImuPlugin::addNoise(Eigen::Vector3d* linear_acceleration,
                               Eigen::Vector3d* angular_velocity,
                               const double dt) {
  // CHECK(linear_acceleration);
  // CHECK(angular_velocity);
  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.

This file has been truncated. show original

heinzketchupp · November 5, 2019, 5:04pm

As your suggestion, this is what I did:

In iris.sdf file, I deleted the gpsNoise variable to eliminate the GPS noise.
In imu,magnetic,mavlink.cpp file, I basically tried to neglect noises by assigning for example:
(*angular_velocity)[i] = (*angular_velocity)[i] ; // + noise
In iris.world, I assigned the magnetic field equals 0.

When I rebuild it and run HITL simulation, I got error:
“Unable to start mission: vehicle not ready”
and the value of Alt in QGC didn’t appear.

Could you give me another suggestion?

JulianOes · November 6, 2019, 8:30am

If there is no noise present the sensor values might be considered stale (or stuck) and ignored in some ekf or sensor voting steps. I suppose you need to debug where the information flows through and what happens step by step.

Edit:
One way to do this is by looking at the various published messages in PX4 by using listener msg_name in the console.

heinzketchupp · November 6, 2019, 9:45am

Thank you so much for your support!
This is gonna be a lot of work.
I just have a small and curious question:
Instead of doing what you said like modifying the cpp file, could I just change the value of the noise and bias directly in iris.sdf file? I planned to find the smallest value that the system could work and then live with it.
As my understanding, everytime I change the cpp file, I have to remake it using: make px4_sitl_default gazebo.

JulianOes · November 6, 2019, 10:31am

I wouldn’t change the iris.sdf file because it is auto-generated from the iris_base.xacro and iris.xacro file as far as I know. I generally don’t mind doing an incremental build, it doesn’t take long.

lizhou · April 30, 2020, 7:28am

@heinzketchupp Maybe you already solved the problem. but I have recommend to change the noise parameters in .world file.

Murray_Louw · July 5, 2021, 10:15pm

You need to eliminate these lines in the DataValidator.cpp for a SITL run with no noise.

You can do it like this so it will still work for practical flights:

#ifndef CONFIG_ARCH_BOARD_PX4_SITL // enable noiseless SITL
    /* we got the exact same sensor value N times in a row */
    _error_mask |= ERROR_FLAG_STALE_DATA;
    ret = 0.0f;
#endif // CONFIG_ARCH_BOARD_PX4_SITL

Then just search for where sensor noise is added and comment out those lines in the plugins, like gazebo_imu_plugin and gazebo_barometer_plugin