Orientation calculation using IMU and magnetometer from hector_gazebo_plugins

[gmsanchez]

Hi,

I am trying to set up a simulation environment in order to get a grasp of how these two filters work and how to get an orientation using the magnetometer. I have configured a Husky simulated vehicle robot with a GPS, an IMU and magnetometer from hector_gazebo_plugins. The configuration of each of these simulated sensors is

<gazebo>
  <plugin name="imu_controller" filename="libhector_gazebo_ros_imu.so">
    <robotNamespace>$(arg robot_namespace)</robotNamespace>
    <updateRate>50.0</updateRate>
    <bodyName>base_link</bodyName>
    <topicName>imu/data_raw</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>
    <headingDrift>0.005</headingDrift>
    <headingGaussianNoise>0.005</headingGaussianNoise>
  </plugin>
</gazebo>

<gazebo>
  <plugin name="magnetometer_controller" filename="libhector_gazebo_ros_magnetic.so">
    <alwaysOn>true</alwaysOn>
    <updateRate>50.0</updateRate>
    <bodyName>base_link</bodyName>
    <topicName>imu/mag</topicName>
    <offset>0 0 0</offset>
    <drift>0.0 0.0 0.0</drift>
    <gaussianNoise>0.0 0.0 0.0</gaussianNoise>
    <declination>-9.3748</declination>
    <inclination>-36.7067</inclination>
    <magnitude>0.0000224340</magnitude>  <!-- 22434.0 nT -->
    <referenceHeading>0</referenceHeading>
    <useMagneticFieldMsgs>true</useMagneticFieldMsgs>
  </plugin>
</gazebo>

<gazebo>
  <plugin name="gps_controller" filename="libhector_gazebo_ros_gps.so">
    <robotNamespace>$(arg robot_namespace)</robotNamespace>
    <updateRate>50</updateRate>
    <bodyName>base_link</bodyName>
    <frameId>base_link</frameId>
    <topicName>navsat/fix</topicName>
    <velocityTopicName>navsat/vel</velocityTopicName>
    <referenceLatitude>-31.64053550641323</referenceLatitude>
    <referenceLongitude>-60.67061589856969</referenceLongitude>
    <referenceHeading>0</referenceHeading>
    <referenceAltitude>0</referenceAltitude>
    <drift>0.0001 0.0001 0.0001</drift>
  </plugin>
</gazebo>

then I launch both imu_complementary_filter and imu_filter_madgwick as follows

<?xml version="1.0"?>
<!-- ComplementaryFilter launch file -->
<launch>

  <!-- Filter raw gyro, accel and mag data into a usable orientation -->
  <node pkg="imu_complementary_filter" type="complementary_filter_node"
      name="complementary_filter_gain_node" output="screen">
    <param name="do_bias_estimation" value="true"/>
    <param name="do_adaptive_gain" value="true"/>
    <param name="use_mag" value="true"/>
    <param name="gain_acc" value="0.01"/>
    <param name="gain_mag" value="0.01"/>

    <remap from="/imu/data" to="/complementary/imu/data" />
  </node>

</launch>

and

<?xml version="1.0"?>
<!-- IMU filter Madgwick launch file -->
<launch>

  <!-- Filter raw gyro, accel and mag data into a usable orientation -->
  <node name="imu_filter" pkg="imu_filter_madgwick" type="imu_filter_node" >
  <!-- Hector Gazebo plugins use NWU, Gazebo uses ENU. Not really sure about this.
        Seems that we should set up this parameter as follows:
        1) NWU if we use the magnetometer, because of hector_gazebo_plugins.
        2) ENU if we don't use the magnetometer, because of Gazebo inertial frame. -->
    <param name="world_frame" value="nwu" />
    <param name="use_mag" value="true" />
    <param name="use_magnetic_field_msg" value="true" />

    <param name="declination" value="-9.3748" />
    <param name="yaw_offset" value="0" />
    <param name="publish_tf" value="false" />

    <remap from="/imu/data" to="/madgwick/imu/data" />
  </node>

</launch>

Now, according to hector_gazebo_plugins documentation, the world frame is using NWU convention and these can be tweaked by using the referenceHeading parameter. As you can see, I left them at 0 degrees in both the GPS and magnetometer configuration. The magnetic field values seem to be correct to what is expected (as the NOAA web page says). When the vehicle x-axis is facing North and its y-axis is facing West, the /imu/mag topic output is:

header: 
  seq: 379
  stamp: 
    secs: 7
    nsecs: 760000000
  frame_id: "base_link"
magnetic_field: 
  x: 1.7745176055738856e-05
  y: 2.9296682395335064e-06
  z: 1.3409329839969677e-05
magnetic_field_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

while the NOAA web page says this is the magnetic field that should be measured using that configuration:

Declination ( + E  | - W ): -9.3748°
Inclination ( + D  | - U ): -36.7067°
Horizontal Intensity: 17,985.5 nT
North Comp (+ N  | - S): 17,745.3 nT
East Comp (+ E  | - W): -2,929.7 nT
Vertical Comp (+ D  | - U): -13,409.2 nT
Total Field: 22,434.0 nT

if I understand this correctly, the NOAA reference frames uses NED and because of that it would be right to expect the changed signs for the yz-axis of the /imu/mag topic (which I assume uses NWU too).

To sum up, using this setup both of the filters provide an initial orientation equal to the declination at startup (in this case, -9.3748) and then they keep this offset while the vehicle moves. If the vehicle starts with its x-axis facing North, shouldn't the initial orientation obtained by the filters be 0 degrees?

Thanks in advance!