Ask Your Question
1

robot_localization with GPS,IMU and odometry , in Gazebo simulation

asked 2020-11-13 21:08:48 -0500

atas gravatar image

updated 2020-12-09 05:41:31 -0500

Hello, I have been working to configure the dual_ekf_navsat setup from robot_localization(brilliant piece of software) for using it with my simulated robot in Gazebo,
I have all sensors, thanks to gazebo_plugins, I have ;

  • Ackermann drive plugin that also provides odom
  • IMU plugin provides imu/data
  • GPS sensor plugin provides gps/fix

I have identical sensor setup with the basic example provided in dual_ekf_navsat, but so far for some reason the second instance of EKF that is responsible for GPS, shifts dramatically and finally explodes. The first EKF instance responsible for continues odom->base_link transform seems doing fine. I really don't know what I am missing here, I checked all the topics names and data published under these topics. Everything is in simulation , so I will expect it to work ideally. y dual_ekf_navsat.yaml is ;

# For parameter descriptions, please refer to the template parameter files for each node.

ekf_local_filter_node:
  ros__parameters:
    use_sim_time: true
    clear_params: true
    publish_tf: true
    filter_type: "ekf"
    frequency: 30.0
    sensor_timeout: 0.1
    odom0: /odometry/wheel
    imu0: /imu/data
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom
    map_frame: map
    odom0_config: [false,  false, false, # X , Y , Z
                    false, false, false, # roll , pitch ,yaw
                    true,  true,  true,  # dX , dY , dZ
                    false, false, false, # droll , dpitch ,dyaw
                    false, false, false] # ddX , ddY , ddZ
    odom0_relative: false
    odom0_differential: false
    odom0_queue_size: 10
    imu0_config: [false,  false, false,  # X , Y , Z
                  false,  false,  true,  # roll , pitch ,yaw
                  false,  false, false,  # dX , dY , dZ
                  false,  false,  true,  # droll , dpitch ,dyaw
                  false,  false,  false] # ddX , ddY , ddZ
    imu0_relative: false                
    imu0_differential: false
    imu0_queue_size: 10
    imu0_remove_gravitational_acceleration: true
    process_noise_covariance: [0.03, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                0.0,    0.03, 0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                0.0,    0.0,    0.04, 0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                0.0,    0.0,    0.0,    0.03, 0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                0.0,    0.0,    0.0,    0.0,    0.03, 0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                0.0,    0.0,    0.0,    0.0,    0.0,    0.06, 0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.025, 0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.025, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                0.0,    0.0,    0 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2020-12-09 04:41:21 -0500

Tom Moore gravatar image

updated 2020-12-11 02:28:32 -0500

Your IMU data is provided in the imu_link frame and your GPS data is in the gps_link frame. Do you have transforms defined from those frames to the robot's base_link frame?

Also, your only reference for Z or its velocity is your odometry data, and you're fusing absolute Z, rather than Z velocity. So that means

  1. Tier 2 EKF (map frame EKF) gets an odometry measurement. frame_id on the pose data in that measurement is odom
  2. EKF attempts to transform from odom->map, because all pose data needs to be transformed into its world_frame. However, that instance of the EKF itself is generating that transform. I'm not totally convinced this is your issue, but it is an issue.

What I would recommend is that your turn off your GPS data as an input to the T2 EKF. See if its state estimate still explodes and report back. Exploding state estimates/covariance usually means some dimension is not being measured.

EDIT in response to comments:

If your two state estimates align perfectly when you don't fuse GPS data, then the inclusion of the GPS data is obviously causing a problem. Can you bag the sensor data, and then play back the bag against a live EKF? Then you can step through each measurement and identify the one that causes the state estimate to explode.

Also, what do you mean by explode? What is the EKF output after an 'explosion'? Do the covariances increase massively?

edit flag offensive delete link more

Comments

Hello @Tom Moore, thanks for your answer, actually I resolved explosion problem.

But now I have one issue with navsat_transfrom_node, when i convert GPS way points to map frame, the points are sometimes hugely off, maybe I should open another question but if you have any rough thoughts on possible reasons I would appreciate.

atas gravatar image atas  ( 2020-12-09 04:56:39 -0500 )edit
1

You should probably open another question. Provide sample GPS messages that lead to good poses, and sample GPS messages that lead to the erroneous ones.

Tom Moore gravatar image Tom Moore  ( 2020-12-09 05:00:11 -0500 )edit

Oh, actually.. I still hit on this time to time. I turned of the GPS to second EKF instance at that case map and odom are perfectly aligned with each other. I updated my latest config file in the original question.

atas gravatar image atas  ( 2020-12-09 05:39:17 -0500 )edit

I also think that the GPS is obviously source of the problem here... , by explosion what I mean is; the odometry/gps, initially will be aligned with odometry/global but after I move robot a little bit then suddenly the pose coming from odometry/gps will jump to over 1e6+ ,a hugely off value. And the covariance also becomes enourmous. I will actually try to use another GPS plugin, maybe this is caused by the gazebo plugin for the GPS I use. Thank you for taking time to reply, I will update here and hopefully I will get feedback from you.

atas gravatar image atas  ( 2020-12-11 09:22:10 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2020-11-13 21:08:48 -0500

Seen: 268 times

Last updated: Dec 11 '20