imu noise of mir

[narutojxl]

Hello authors,
Thanks for your sharing this interesting software. I'm using your mir_robot noetic branch. When remaining the default imu noise params <xacro:property name="imu_stdev" value="0.00017" /> of mir_100_v1.urdf.xacro, which loads imu.gazebo.urdf.xacro and sets <xacro:imu_gazebo link="${prefix}imu_link" imu_topic="/imu/data" update_rate="50.0" stdev="${imu_stdev}" />. imu's gaussianNoise =0.00017 * 0.00017 is very very small.

• This is imu.gazebo.urdf.xacro content:

<?xml version="1.0"?>

<robot xmlns:xacro="http://ros.org/wiki/xacro">
  <!-- If tf_prefix is given, use "frame tf_prefix/imu_frame", else "imu_frame" -->
  <xacro:arg name="tf_prefix" default="" />
  <xacro:property name="tf_prefix" value="$(arg tf_prefix)" />
  <xacro:if value="${tf_prefix == ''}">
      <xacro:property name="imu_frame" value="imu_frame" />
  </xacro:if>
  <xacro:unless value="${tf_prefix == ''}">
      <xacro:property name="imu_frame" value="$(arg tf_prefix)/imu_frame" />
  </xacro:unless>

  <xacro:macro name="imu_gazebo" params="link imu_topic update_rate stdev">
    <gazebo reference="${link}">
      <gravity>true</gravity>
      <sensor name="imu_sensor" type="imu">
	<always_on>true</always_on>
	<!--update_rate>100</update_rate-->  <!-- this is not the publish rate -->
	<visualize>true</visualize>
	<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
	  <topicName>${imu_topic}</topicName>
	  <bodyName>${link}</bodyName>
	  <frameName>${link}</frameName>  <!--default: imu_frame, from real MiR -->
	  <updateRateHZ>${update_rate}</updateRateHZ>  <!-- this is the publish rate -->
	  <gaussianNoise>${stdev * stdev}</gaussianNoise>
	  <xyzOffset>0 0 0</xyzOffset>
	  <rpyOffset>0 0 0</rpyOffset>
	  <initialOrientationAsReference>true</initialOrientationAsReference>         
	</plugin>
	<pose>0 0 0 0 0 0</pose>
      </sensor>
    </gazebo>
  </xacro:macro>
</robot>

• I launch mir with empty.world and send velocity rostopic pub /cmd_vel geometry_msgs/Twist -r 3 -- '[0,0,0]' '[0, 0, 0.4]' to let mir rotate in place. I found the imu's acc and gyro_z are very noisy. First picture is acc_x and acc_y,
  second is acc_z, third is gyro_x and gyro_y, the last one is gyro_z. For example, acc_x often is 2 m/s^2, acc_y often is 1m/s^2, acc_z often is 8.5 to 11 m/s^2. Command angular z velocity is 0.4 rad/s, but the gyro_z is 0.36 rad/s to 0.42 rad/s.
• Why imu noise so small, but the measured by imu is very noisy, gazebo_ros_imu_sensor.cpp#L112-L123, i don't figure out this. Is this phenomenon normal, thanks for your any advice and help.

[mintar]

Almost all of that noise does not come from the gaussianNoise parameter, but is unintentional. Here's a plot of your experiment (rotating robot) where I set gaussianNoise = 0:

It looks almost identical to your plot. The noise is the same when the robot is stationary. I think this is because even when the robot is stationary in Gazebo, there are always small corrections being applied. There are a lot of simulated forces at work: there is a repulsive force between the ground and each of the wheels; the powered wheels are being commanded to turn forwards and backwards to keep them still, and so on. All of this leads to the robot to "vibrate" by a minuscule amount. Unfortunately, the gazebo_ros_imu_sensor implementation just reads the instantaneous linear accelerations and angular velocities and returns those. What it should do is calculate the mean for each sampling period (the plugin is updated at 1000 Hz, but only publishes at 50 Hz).

Apart from that, the gazebo_ros_imu_sensor implementation of noise is just plain horrible. There's only one parameter (gaussianNoise) that's applied equally to orientation (as a unitless quaternion), linear acceleration (in m/s²) and angular velocity (in rad/s). Instead, it should at least have these parameters:

• gyroscope white noise N_g, rad/√s (noise density)
• acceleration white noise N_a, (m/s)/√s (noise density)

... and possibly also:

• gyroscope random walk K_g, (rad/s)/√s (random walk)
• acceleration random walk K_a, (m/s²)/√s (random walk)

Then the total noise can be properly calculated like this:

import numpy as np

# n: number of measurements
# dt: sampling rate [s], e.g. dt = 1e-2 when sampling at 100 Hz
def generate_signal(n, dt, noise_density, random_walk, random_state=0):
    rng = np.random.RandomState(random_state)
    white = noise_density / np.sqrt(dt) * rng.randn(n)
    walk = random_walk * np.sqrt(dt) * np.cumsum(rng.randn(n))
    return white + walk

The orientation should probably be either not filled at all or be returned without noise, because in order to be realistic, it should be coupled to the gyroscope and accelerometer noise. There are already packages like imu_filter_madgwick that compute the orientation from gyro and accelerometer data, but it doesn't make sense to replicate that here.

So in short: If you want to get rid of the noise, you'll have to improve gazebo_ros_imu_sensor!

[narutojxl]

Hi @mintar, thanks for your very kind and detailed reply!
Without adding allan variance noise(noise density and random walk) and without adding gaussianNoise, according to your advance, i calculate acc and gyro by averaging for each sampling period, the noise is smaller than the instantaneous ones as we expected, although it still is a little noisy. I agree with you the noisy data is caused by gazebo simulating previously mentioned forces.
There is also a interesting thing, we send command is vx = 0, wz=0.4 rad/s, but the wz's mean is 0.425 rad/s measured by imu.
Thanks again! Here are some test pictures. The order is acc_x_&&_acc_y, acc_z, wz.

[mintar]

Maybe you should try this IMU plugin instead:

• http://wiki.ros.org/hector_gazebo_plugins#GazeboRosImu
• https://github.com/tu-darmstadt-ros-pkg/hector_gazebo/blob/melodic-devel/hector_gazebo_plugins/src/gazebo_ros_imu.cpp

It looks much better!

If you successfully use it, please report back!

[narutojxl]

Hi @mintar, thanks for your suggestions. I compare mir and husky, and compare libgazebo_ros_imu_sensor.so and libhector_gazebo_ros_imu.so with the same noise param. Because huksy wheel skidding when performing rotation in place, see here, so in the following 4 tests, i only send rostopic pub /cmd_vel geometry_msgs/Twist -r 3 -- '[0.4,0,0]' '[0, 0, 0]' to let robot forward.

• The order of following 2 pictures is mir_gazebo_ros_imu_sensor, and then husky_gazebo_ros_imu_sensor. The imu plugin param for two tests are the same: 50hz, gaussianNoise = 0.005. From the result we can see with the same imu plugin and the same params, hushy model's imu(acc, gyro) noise is smaller than mir. There is no big difference of imu noise when husky is stationary or moving. But for mir model, when mir moving, its imu's noise is bigger than it is stationary. When i change imu plugin to hector_gazebo_ros_imu for mir and husky, i can also get the same conclusion.

• The order of following 2 pictures is mir_gazebo_ros_imu_sensor, and then mir_hector_gazebo_ros_imu. The imu plugin param for two test are the same: 50hz, gaussianNoise = 0.005. We can see on the same mir model, hector_gazebo_ros_imu noise is smaller than gazebo_ros_imu_sensor. When using huksy model, we can also get the same conclusion.

• So with the same imu noise params config, on the husky model with hector_gazebo_ros_imu imu plugin, we can get smallest imu noise result. I'm wonder two things. 1: Why mir model noise bigger than huksy with the same imu plugin? 2: Why mir imu noise in motion state is very different from in stationary state, the husky model not have this phenomenon?
• Here are 4 tests imu plugin config.
• mir gazebo_ros_imu_sensor, stdev = 0.0707, 0.005 = 0.0707 * 0.0707:

 <xacro:macro name="imu_gazebo" params="link imu_topic update_rate stdev">
    <gazebo reference="${link}">
      <gravity>true</gravity>
      <sensor name="imu_sensor" type="imu">
	<always_on>true</always_on>
	<update_rate>100</update_rate>  <!-- this is not the publish rate -->
	<visualize>true</visualize>
	<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
	  <topicName>${imu_topic}</topicName>
	  <bodyName>${link}</bodyName>
	  <frameName>${link}</frameName>  <!--default: imu_frame, from real MiR -->
	  <updateRateHZ>${update_rate}</updateRateHZ>  <!-- this is the publish rate -->
	  <gaussianNoise>${stdev * stdev}</gaussianNoise>
	  <xyzOffset>0 0 0</xyzOffset>
	  <rpyOffset>0 0 0</rpyOffset>
	  <initialOrientationAsReference>false</initialOrientationAsReference>         
	</plugin>
	<pose>0 0 0 0 0 0</pose>
      </sensor>
    </gazebo>
  </xacro:macro>

• mir hector_gazebo_ros_imu

<xacro:macro name="imu_gazebo" params="link imu_topic update_rate stdev">
  <gazebo>
    <plugin name="imu_controller" filename="libhector_gazebo_ros_imu.so">
	<updateRate>${update_rate}</updateRate>
	<bodyName>${link}</bodyName>
	<frameId>${link}</frameId>
	<topicName>${imu_topic}</topicName>
	<accelDrift>${stdev*stdev} ${stdev*stdev} ${stdev*stdev}</accelDrift>
	<accelGaussianNoise>${stdev*stdev} ${stdev*stdev} ${stdev*stdev}</accelGaussianNoise>
	<rateDrift>${stdev*stdev} ${stdev*stdev} ${stdev*stdev} </rateDrift>
	<rateGaussianNoise>${stdev*stdev} ${stdev*stdev} ${stdev*stdev} </rateGaussianNoise>
	<yawDrift>${stdev*stdev}</yawDrift> 
	<yawGaussianNoise>${stdev*stdev}</yawGaussianNoise>
	<gaussianNoise>${stdev*stdev}</gaussianNoise>
    </plugin>
  </gazebo>
</xacro:macro>

• husky gazebo_ros_imu_sensor

  <gazebo reference="base_link">  <!--the output data(acc,gyro,quat) in this frame, this frame should consistent with imu box's axis--> 
    <gravity>true</gravity>
    <sensor name="imu_controller" type="imu">
      <always_on>true</always_on>
      <visualize>true</visualize>
      <plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
	<topicName>imu/data</topicName>
	<bodyName>base_link</bodyName>    <!--where's the imu box origin -->
	<frameName>base_link</frameName>  <!--topic's frame_id, should be consistent with reference param-->
	<updateRateHZ>50.0</updateRateHZ> <!--topic hz, should be consistent with update_rate param-->
	<gaussianNoise>0.005</gaussianNoise>
	<xyzOffset>0 0 0</xyzOffset>
	<rpyOffset>0 0 0</rpyOffset>
	<initialOrientationAsReference>true</initialOrientationAsReference> <!--true: init quat is [0,0,0,1]-->
      </plugin>
      <pose>0 0 0 0 0 0</pose>
    </sensor>
  </gazebo>

• husky hector_gazebo_ros_imu

  <gazebo>  
      <plugin name="imu_controller" filename="libhector_gazebo_ros_imu.so">
	<updateRate>50</updateRate>
	<bodyName>base_link</bodyName>
	<frameId>base_link</frameId>
	<topicName>/imu/data</topicName>
	<accelDrift>0.005 0.005 0.005</accelDrift>
	<accelGaussianNoise>0.005 0.005 0.005</accelGaussianNoise>
	<rateDrift>0.005 0.005 0.005</rateDrift>
	<rateGaussianNoise>0.005 0.005 0.005</rateGaussianNoise>
	<yawDrift>0.005</yawDrift> 
	<yawGaussianNoise>0.005</yawGaussianNoise>
	<gaussianNoise>0.005</gaussianNoise>
      </plugin>
  </gazebo>

[mintar]

Thank you for the detailed analysis! I've debugged and solved the problem in commit 063d4e7. The problem was that I used STL meshes as collision geometries for the wheels. If you visualize contacts in Gazebo, you can see that there are lots of inconsistent contacts (not all 6 wheels touch the ground at all times), which lead to these simulated "vibrations" of the robot, which was picked up by the simulated IMU:

mir.mp4

The husky doesn't have this problem:

husky.mp4

After replacing the collision geometries with cylinders, the problem is fixed for the MiR:

mir_fixed.mp4

[mintar]

By the way, you should use -r 10 (or larger) instead of -r 3 in your rostopic pub calls, otherwise the Husky will brake and accelerate repeatedly.

[mintar]

I've also switched to the hector_gazebo_plugins now.

[narutojxl]

Hi author @mintar, thanks very much :)