improve IMU downsampling

[abaeyens]

Desired behavior

The IMU signal behaves similarly to what one would expect on a real-world IMU. Some noise due to erratic movements of the robot, but nothing large. Example (produced with patched Gazebo):

Observed behavior

The IMU signal is noisy AF, not at all representative of the actual movements of the robot. Example, and please note the difference between the y-axis scale of this graph and the previous one (produced with current Gazebo Harmonic):

In the example above, the IMU frequency was set to 5 Hz and the simulation step period to 3 ms.

Reason for the observed behavior

Note: I'm new to the Gazebo source code, so please correct me where I'm wrong!

Gazebo calculates accelerations and velocities for each object instance at each timestep. For the IMU sensor, it takes the values of these every n steps, with n = simulation_step_rate / sensor_readout_rate and publishes them.

For many sensors, taking the instantaneous value makes sense. However, for the angular velocity and linear acceleration of an IMU it does not, as real-life IMU's don't work like that. In real-life IMU's, the physically occurring velocities and accelerations get low-pass filtered (on the analog and/or digital level) before being published at a low rate. See for example the datasheet of the popular BMI088, section 4.4: for an output rate (ODR) of 12.5 Hz, the analog signal gets low-pass filtered to 5 Hz (3 dB cutoff frequency).

This explains the mismatch between expected and observed behavior: in the Gazebo implementation, severe aliasing happens due to lack of low-pass filtering before downsampling, showing up as noise.

This results in two issues:

• Inner simulation workings show up clearly in the sensor values. The step-to-step noise on the accelerations and velocities calculated by Gazebo varies from situation to situation and depends strongly on simulation settings. On itself that's a non-issue, however, this noise must not show up that severely in sensor readings. Applying a low-pass filter filters out most of this, as the sensor readout frequency tends to be a good bit lower than the simulation step rate.
• High-frequency sensor movement gets aliased on the sensor readings. Even if Gazebo's accelerations and velocities would be perfect (with the noise mentioned above not present), then still the simulated reading wouldn't match well what a real-life IMU sensor would give, because that real-life sensor applies a low-pass filter, avoiding sampling issues. Applying a low-pass filter appropriate for the output data rate avoids this downsampling issue.

Implementation suggestion

As I believe that this is an important feature for anyone using the IMU sensor, I'm interested in making PRs for this. I'll need some guidance though, as I'm not familiar with the Gazebo codebase and the made assumptions.

Main proposed changes:

• In src/ImuSensor.cc
  • Add a method

    void ImuSensor::FilterAndSetAngularVelocityAndLinearAcceleration(
      const math::Vector3d &_angularVel,
      const math::Vector3d &_linearAcc,
      const std::chrono::steady_clock::duration _simTime,
      const std::chrono::steady_clock::duration _readOutPeriod)

    which updates the velocities and accelerations while applying a low-pass filter, like

    this->dataPtr->linearAcc = this->dataPtr->linearAcc * beta + _linearAcc * alpha;
    

    with alpha + beta == 1.
  • Add a class variable prevSetStep which keeps track of the last time the above lowpass filter was run.
  • In the ImuSensor::Update method, which publishes the Imu message, make a copy of the accelerations and velocities and apply the gravity and the noise on the copies, such that the existing class variables can be used for the low-pass filtering. (These math::Vector3 objects are small, so the performance impact of a copy is negligible.)
• In gz-sim:src/systems/imu/imu.cc
  • In the method Imu::PostUpdate, always update the IMU sensor such that the low-pass filter is run. This makes the simulation slightly slower, but I think the resulting fidelity increase makes this change more than worth it (and compared to all the rendering and physics calculations, the overall impact of running this lowpass filter is probably not measurable).
  • The method ImuPrivate::Update gets extended with a second argument for the simulation time:

    public: void Update(const EntityComponentManager &_ecm,
       const std::chrono::steady_clock::duration _simTime);

  • And in that method, we call ImuSensor::FilterAndSetAngularVelocityAndLinearAcceleration instead of the current ImuSensor::SetAngularVelocity and ImuSensor::SetLinearAcceleration. We include the simulation time such that the low-pass filter can calculate the appropriate alpha.
• Add to the IMU sensor a parameter lowpass_cutoff_frequency_ratio that defaults to 2.5. This is the ratio between the sensor output rate and the 3 dB frequency of the applied low-pass filter. This default matches with what's used in the BMI088 (and likely many other similar devices) and makes sense from a signal processing perspective, providing a reasonable balance between avoiding aliasing without smoothing out and delaying the signal too much.
• And some changes here and there for GUI, SDF etc., haven't looked into that yet in detail.

I'm not sure what Gazebo's way is of handling changes over multiple repositories and avoiding the resulting compatibility conflicts. The proposed above changes in gz-sim depend on those in gz-sensors, though the other way round there's no compatibility issue.

I'd appreciate feedback on the proposed changes.

[azeey]

in the Gazebo implementation, severe aliasing happens due to lack of low-pass filtering before downsampling, showing up as noise

I think you're absolutely right! Thanks for catching this. cc @mjcarroll to confirm.

Your suggested changes would definitely work, I think it would be easier to low pass filter the signal in the Imu system in gz-sim instead of doing it in gz-sensors. That way, we only need to make changes in gz-sim.