How to properly fuse IMU + Barometer + ToF Ranger? (Robot_Localization)

Question

I'm having difficulties getting a good absolute Z height. This is a flying vehicle with no GPS.

Currently my ToF sensor pointed down reads 18cm (which is accurate to where my FC/ToF is mounted) but my ekf height is around 60cm and starts exponentially climbing. I want the ToF to be a hard sensor jump when read, but say I move over something I don't want the height to just jump up, which is why I wanted to fuse IMU, Baro, and ToF so that it could for the most part handle those errors.

I have have MAVROS (ArduPilot) send me IMU, Barometer, and Compass data to my Companion Computer. From there I fuse IMU + Compass using imu_filter_madgwick to get a solid Yaw reading.

If you're wondering why I don't just have ArduPilot handle this... it's because it doesn't work, no matter what settings I've configured in Mission Planner/qGroundControl it never uses the Compass to correct it's yaw.

Side Question: How does robot_localization get that data? madgwick outputs a sesnsor_msgs/IMU type which has a quaternion orientation field, does r_l auto convert to RPY for the true/false matrix?

madgwick_node = Node (
    package='imu_filter_madgwick',
    executable='imu_filter_madgwick_node',
    name='imu_mag_ekf',
    parameters=[{
        'world_frame': "enu",
        'use_mag': True,
        'use_magnetic_field_msg': True,
        'remove_gravity_vector': False,

    }],
    remappings=[
        #('/imu/data_raw', '/mavros/imu/data_raw'),
        ('/imu/data_raw', '/mavros/imu/data'),
        ('/imu/mag', '/mavros/imu/mag'),
        ('/imu/data', '/vehicle/madgwick_ekf')
    ],
    emulate_tty=True,
    output='screen',
)

Then I push the IMU + Compass to robot_localization:

/ekf_filter_node:
ros__parameters:
frequency: 20.0
sensor_timeout: 0.2
publish_tf: true
two_d_mode: false
print_diagnostics: false #echo the /diagnostics_agg topic for details

smooth_lagged_data: true
history_length: 3.0

map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: map



# Barometer
pose0: /vehicle/altitude
pose0_config: [false, false, true,
              false, false, false,
              false, false, false,
              false, false, false,
              false, false, false]
pose0_differential: true
# pose0_relative : false

# ToF
pose1: /vehicle/lidar
pose1_config: [false, false, true,
              false, false, false,
              false, false, false,
              false, false, false,
              false, false, false]
# pose1_differential: true
# pose1_relative : false

# IMU and Compass Fused EKF from Madgewick Node
imu0: /vehicle/madgwick_ekf
imu0_config: [false, false, false, # X, Y, Z
              true,  true,  true, # roll, pitch, yaw
              false, false, false, # Linear velocities
              true, true, true, # angular velocity
              false, false, true] # linear acceleration
# imu0_remove_gravitational_acceleration: true
# imu0_queue_size: 5
# imu0_nodelay: false
# imu0_differential: false
# imu0_relative: true



# imu1: /mavros/imu/data_raw
# imu1_config: [false,  false,  false,
#               false,  false,  false,
#               false,  false,  false,
#               true,   true,   true,
#               true,   true,   true]
# imu1_differential: true
# imu1_remove_gravitational_acceleration: true

I've read over tons of threads from over the years and lots of documentation but I am struggling to actually piece it together. One issue I know is that I don't really understand how the TF goes into play.

• map_frame: map
  • I don't know
• odom_frame: odom
  • How the vehicle POSE relates to the 2D map?
• base_link_frame: base_link
  • Actual vehicle in 3D, POSE and TWIST
• world_frame: map
  • I don't know

I often read things like "you need a map->odom transformation" which makes zero sense. Wouldn't I want to translate my odom data to it's position on the map, not how the map relates to the odom? This pattern seems to continue for all talk about TF.

Well at least for me I don't need or plan on using a map. So I really just care about understanding 'base_link' and 'odom' for the time being. I've tried disabling 'map_frame' and setting 'world_frame' to 'odom' but that didn't solve any of my issues.

Answer 1

does r_l auto convert to RPY for the true/false matrix?

Yes, r_l deals in Euler angles internally.

I often read things like "you need a map->odom transformation" which makes zero sense.

It sounds to me that you may not have a firm grasp of coordinate frames in ROS. For that, I recommend reading REP-105 (and it can't hurt to review REP-103).

One issue I know is that I don't really understand how the TF goes into play

I assume you've read through the tf (really tf2) wiki?

To answer your original question of why your state estimate is exploding, you can look at your sensor configs for the answer.

• For your /vehicle/altitude topic, you are fusing only Z position
• For your /vehicle/lidar topic, you are fusing only Z position
• For your /vehicle/madgwick_ekf, you are fusing absolute orientation and rotational velocity

Your EKF is estimating 3D pose (X, Y, Z, roll, pitch, yaw), 3D velocities (the velocities for the 3D pose variables) and linear acceleration. Yet you are only providing measurements for Z position, absolute orientation, and rotational velocity.

An EKF has two stages: prediction and correction. In the prediction stage, it has to predict, given the current state, the next state. When it does that prediction, the covariance for all predicted variables grows. When we receive measurements, we constrain that covariance growth.

As you are not providing any measurements for most of the variables in the EKF, the covariance in those unmeasured variables will continue to grow without bound. The correlation between state variables (as defined in the transfer function) will mean that small changes in Z will result in massive changes in X and Y, which will in turn have effects on Z and its velocity.

TL;DR, you need to provide either absolute position for X and Y or their velocities (doing both is even better).

If you could include sample messages from every sensor input, that would also indicate if you have any other issues with your configuration.