search

gazebosim / gazebo-classic

Gazebo classic. For the latest version, see https://github.com/gazebosim/gz-sim
http://classic.gazebosim.org/
Other
1.15k stars 478 forks source link

Improve sensor noise modeling #1285

Open osrf-migration opened 9 years ago

osrf-migration commented 9 years ago

Original report (archived issue) by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

I saw a talk by @asymingt recently in which he discussed improved noise modelling in gazebo, especially for GNSS (GPS) and IMU sensors. He has already implemented much of this in gazebo plugins in his crates repository.

Some brief history:

@asymingt indicated in preliminary discussions that "time-correlated error and axis coupling" should be accommodated. This would probably require some additional sdformat element definitions. Here's a link to the sensor noise parameters for the crates hummingbird model.

osrf-migration commented 9 years ago

Original comment by Andrew Symington (Bitbucket: asymingt).

Thanks for spearheading this conversation!

My comments were mainly motivated by working with data fusion, localization and GNSS modelling. Gazebo has been an extremely useful tool for CRATES, which is a work in progress: it's currently undergoing a refactor, while I push whatever the Gazebo project finds useful upstream.

While developing CRATES what I noticed however, is that the following sorts of things are quite difficult to model in Gazebo using default <sensor> options:

The thing that links all of these is that the noise is correlated in space and time: in other words the current error perturbation is related to (not independent from) to perturbations drawn at locations close by, or at epochs close to each other in time.

Having looked through the Gazebo source a bit, what I'd like to tentatively propose are the following three usage patterns for error modelling:

  1. Easy: Basic assumption is a deterministic measurement. Not very realistic, but useful if you are trying to validate some analytic model of the world.
  2. Intermediate: Standardised <noise> tag which provides independently-drawn error perturbations, or simple time-correlated perturbations (see below).
  3. Advanced: A custom SensorPlugin, in which sensor signals may be correlated with each other, or with some other quantity (examples: its own or another entity's location, temperature, humidity, pressure, ionospheric conditions, etc. These would be distributed as separate ROS packages providing SensorPlugins and WorldPlugins , dependent on gazebo_ros_pkgs. This also has the benefit that Gazebo itself need not link against many sensor-specific third party libraries.

The course of action I have chosen is to first propose Easy / Intermediate implementations for sensors not already described in SDF / supported by Gazebo. Namely, magnetometers in issue #1294 and altimeters in issue #1297. I also proposed a nomenclature update from GPS to GNSS in issue #1297.

What I would like to do next is propose that the Ornstein-Uhlenbeck distribution be supported in the <noise> tag. It's a good start for modelling noise that drifts with time, but periodically reverts back to a mean quantity. It may be useful as a model for short-term IMU orientation error (fast corrections of integrated gyro readings) or long-term barometric altimeter error (slow changes in atmospheric pressure).

On a related note, we also need to think (a) whether all sensors can be supported by a common <noise> tag / set of distributions, and (b) whether a <noise> tag should be defined for each sensor signal. What I mean by this is should, for example, GPS position_x, position_y and position_z have separate noise distributions, because typical satellite vehicle geometry with respect to the receiver position causes a much greater error in Z than in X or Y.

Once the Basic/Intermediate framework is in place, I can repackage my code in CRATES as a separate ROS package (providing SensorPlugins and WorldPlugins) with a gazebo_ros_pkgs dependency.

I'm almost certain @SJulier will have some valuable input to this conversation.

osrf-migration commented 9 years ago

Original comment by Andrew Symington (Bitbucket: asymingt).

@scpeters, I've coded up Wiener and Ornstein-Uhlenbeck noise models along the same lines as the Gaussian noise model already in Gazebo. However, since these are stochastic noise processes, I needed to modify the Noise::Apply and Noise:ApplyImpl prototype functions to accept a time epoch for the noise perturbation:

    /// \class Noise Noise.hh
    /// \brief Noise models for sensor output signals.
    class GAZEBO_VISIBLE Noise
    {

      ...

      /// \brief Apply noise to input data value.
      /// \param[in] _in Input data value.
      /// \param[t] _t Elapsed simulation time
      /// \return Data with noise applied.
      public: double Apply(double _in, const common::Time &_t = common::Time::Zero);

      /// \brief Apply noise to input data value. This gets overriden by
      /// derived classes, and called by Apply.
      /// \param[in] _in Input data value.
      /// \param[t] _t Elapsed simulation time
      /// \return Data with noise applied.
      public: virtual double ApplyImpl(double _in, const common::Time &_t);

      ...

};

I'm not yet ready to do a pull request, but you can see my work in progress here:

My intention is to also write unit tests for both models, but I seem to be getting stuck every time at this point (for both Wiener and Ornstein)

  // Make sure that we have a valid noise model
  sensors::OrnsteinNoiseModelPtr noiseModel =
      boost::dynamic_pointer_cast<sensors::OrnsteinNoiseModel>(_noise);
  ASSERT_TRUE(noiseModel);

For some reason noiseModel is always evaluated as false.

Finally, we need to think whether or not it makes sense to include the epoch inside the noise callback for custom types. I didn't change this, because I imagine that there already exist many SensorPlugins which use the current callback that takes only the current sensor value _in. Including a second epoch argument _t would likely break compatibility, which is a bad thing...

osrf-migration commented 9 years ago

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

I'll try building your branches to see if I can reproduce the unit test failure.

I'll have to think about the ApplyImpl issue...

osrf-migration commented 9 years ago

Original comment by Andrew Symington (Bitbucket: asymingt).

Thanks!

We could get around the issue by allowing child classes of Noise to observe the world directly, and hence access global time. This would allow us to retain a single argument _in to both the callback and Apply / ApplyImpl.

A semi-related issue that I also foresee is that a SensorPlugin seems to have no access to the world, according to API 4.0.0. This will present a similar problem if one wishes to implement a different noise model that requires knowledge of simulated time.

osrf-migration commented 9 years ago

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

There is a way to get a world pointer from a SensorPlugin (see PressurePlugin::Load), but it is a bit tortured.

osrf-migration commented 9 years ago

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

The test failure may be caused by a missing virtual keyword before the ApplyImpl in the new *NoiseModel classes. I'm testing a fix. Nope

osrf-migration commented 9 years ago

Original comment by Andrew Symington (Bitbucket: asymingt).

By the way, in case you are interested, here is a little diagram illustrating the difference between the three noise processes -- Gaussian, (drift-free) Wiener and Ornstein-Uhlenbeck. Note how the O-U process reverts periodically back to its mean of 10, while the Wiener process is really just a totally random walk.

noiseprocesses.png

It was generated using the MATLAB code below, which I used to code the processes up in C++ for Gazebo.

r = 10;     % Reversion time (O-U)
m = 10;     % Mean (O-U, Gaussian, Wiener)
x = m;      % Starting estimate (O-U, Wiener)
o = 0;      % Bias offset (for Wiener) 
v = 2;      % Variance (O-U, Wiener, Gaussian)
T = 100;    % Time
N = 1000;   % Epochs
d = T/N;    % deltaT
M = 1000;   % Samples

% For storing sample traces
s_g = m + sqrt(v) * randn(1,N);
s_w = zeros(1,N);
s_o = zeros(1,N);

% Wiener
b = zeros(1,M);
for j = 1:M
    s_w(1) = x;
    for i = 2:N
        s_w(i) = s_w(i-1) + d*o + sqrt(d*v) * randn();
    end
    b(j) = s_w(end);
end
disp('WIENER PROCESS');
disp('*********************************');
disp('Measured vs analytic mean:');
[mean(b) x+o*T]
disp('Measured vs analytic deviation:');
[var(b)  v*T]

% Ornstein-Uhlenbeck
c = zeros(1,M);
for j = 1:M
    s_o(1) = x;
    for i = 2:N
        k = exp(-d/r);
        s_o(i) = k*s_o(i-1) + (1-k)*m + sqrt(v*(1-k*k)*r/2) * randn();
    end
    c(j) = s_o(end);
end
k = exp(-T/r);
disp('ORNSTEIN-UHLENBECK PROCESS');
disp('*********************************');
disp('Measured vs analytic mean:');
[mean(c) m+(x-m)*k]
disp('Measured vs analytic deviation:');
[var(c)  v*r/2*(1-k*k)]

figure; hold on; grid on;
plot(s_g,'r-');
plot(s_w,'b-');
plot(s_o,'k-');
legend('Gaussian','Wiener','Ornstein-Uhlenbeck');
osrf-migration commented 9 years ago

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

Here's one issue that was causing the test failure:

diff -r d72368b5c665 gazebo/sensors/Noise.cc
--- a/gazebo/sensors/Noise.cc   Mon Oct 20 12:33:03 2014 +0100
+++ b/gazebo/sensors/Noise.cc   Mon Oct 20 13:25:01 2014 -0700
@@ -55,7 +55,7 @@
   }
   else if (typeString == "wiener")
   {
-    noise.reset(new Noise(Noise::WIENER));
+    noise.reset(new WienerNoiseModel());
     GZ_ASSERT(noise->GetNoiseType() == Noise::WIENER,
         "Noise type should be 'wiener'");
   }
osrf-migration commented 9 years ago

Original comment by Andrew Symington (Bitbucket: asymingt).

Great -- works my side. I will debug the remainder of the tests now. Thanks for picking my error up!

osrf-migration commented 9 years ago

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

osrf-migration commented 7 years ago

Original comment by James Goppert (Bitbucket: jgoppert).

Any progress on this? I think it would be great to get random walk processes as part of gazebo built-in noise models.

osrf-migration commented 7 years ago

Original comment by Nate Koenig (Bitbucket: Nathan Koenig).

osrf-migration commented 7 years ago

Original comment by James Goppert (Bitbucket: jgoppert).

I'm going to tackle this. My plan is to add dt as a parameter to noise apply method and then add a new random walk nosie model. My goal is to match the noise model here so it doesn't have to be maintained anymore: https://github.com/ethz-asl/rotors_simulator/blob/master/rotors_gazebo_plugins/src/gazebo_imu_plugin.cpp.

osrf-migration commented 7 years ago

Original comment by James Goppert (Bitbucket: jgoppert).

#!c++

+++ b/gazebo/sensors/Noise.hh   Wed Mar 29 19:31:11 2017 -0400
@@ -59,7 +59,8 @@
       {
         NONE,
         CUSTOM,
-        GAUSSIAN
+        GAUSSIAN,
+        GAUSSIAN_WALK
       };

       /// \brief Constructor. This should not be called directly unless creating
@@ -79,14 +80,16 @@

       /// \brief Apply noise to input data value.
       /// \param[in] _in Input data value.
+      /// \param[in] _dt Elapsed time since last measurement.
       /// \return Data with noise applied.
-      public: double Apply(double _in);
+      public: double Apply(double _in, double _dt);

       /// \brief Apply noise to input data value. This gets overriden by
       /// derived classes, and called by Apply.
       /// \param[in] _in Input data value.
+      /// \param[in] _dt Elapsed time since last measurement.
       /// \return Data with noise applied.
-      public: virtual double ApplyImpl(double _in);
+      public: virtual double ApplyImpl(double _in, double _dt);
osrf-migration commented 7 years ago

Original comment by James Goppert (Bitbucket: jgoppert).

There is also a difference in how noise is handled. rotors currently uses noise density. Noise Density * dt = Variance, so the effect is that if the current gazebo sensors are over sampled, you will be able to achieve more accuracy than actually possible form the sensor and if you under sample, you will have less accuracy, so this probably is better how it is handled in rotors. The issue is that the current parameters all ask for standard deviation, so what you prefer for the new model, standard deviation or noise density. I want to be able to support noise density curently. I could just have the GAUSSIAN_WALK model use noise density and leave the GAUSSIAN model as is. That is probably the most straight forward, but not consistent.

osrf-migration commented 7 years ago

Original comment by James Goppert (Bitbucket: jgoppert).

Another question, where best to recover dt. ImuSensor already computes it using the incoming message. This is probably best, but how to generalize it to the other sensors. Will all sensor msgs have a timestamp, and can I somehow access this timestamp in the sensor base class and set dt for all derived sensors so I don't have to repeat the code?

osrf-migration commented 7 years ago

Original comment by James Goppert (Bitbucket: jgoppert).

PR: https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-requests/2670