Fusing GPS in robot_localization

asked 2022-07-05 23:23:48 -0500

MrOCW
58 ●20 ●28 ●34

updated 2022-07-05 23:50:20 -0500

Hi, I am trying to fuse GPS with all other odom and IMUs My local odometry has barely any drift now. The only issue is that the direction of the GPS odometry seems to be rotated with respect to the other odometry..

image description

you can see that the robot is driving towards the left but the GPS is translating upwards. particular attention to imu0

ekf_filter_node_odom:
  ros__parameters:

    use_sim_time: true

    frequency: 30.0
    sensor_timeout: 0.1
    two_d_mode: false
    transform_time_offset: 0.0
    transform_timeout: 0.0
    print_diagnostics: false
    debug: false

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

    odom0: /wheel/odom
    odom0_config: [false, false, false,
                  false, false, false,
                  true,  true,  true,
                  false, false, true,
                  false, false, false]
    odom0_queue_size: 10
    odom0_nodelay: true
    odom0_differential: false
    odom0_relative: true

    odom1: /visual_slam//odometry
    odom1_config: [true, true, true,
                  true, true, true,
                  false,  false,  false,
                  false, false, false,
                  false, false, false]
    odom1_queue_size: 10
    odom1_nodelay: true
    odom1_differential: true
    odom1_relative: false

    odom2: /lidar/odom
    odom2_config: [true, true, true,
                  false, false, false,
                  false,  false,  false,
                  false, false, false,
                  false, false, false]
    odom2_queue_size: 10
    odom2_nodelay: true
    odom2_differential: false
    odom2_relative: false

    imu0: /imu/with/absolute/orientation/returns 0 when facing East
    imu0_config: [false, false, false,
                true, true, true,
                false, false, false,
                true, true, true,
                false, false, false]
    imu0_nodelay: false
    imu0_differential: false
    imu0_relative: true # IS THIS CORRECT? IF IT IS FALSE, THE DIRECTION OF base_link FACES ANOTHER DIRECTION FROM THE OTHER ODOMETRY SOURCES TRANSLATION DIRECTION
    imu0_queue_size: 10
    imu0_remove_gravitational_acceleration: false

    imu1: /imu1
    imu1_config: [false, false, false,
                false, false, false,
                false, false, false,
                true, true, true,
                false, false, false]
    imu1_nodelay: false
    imu1_differential: true
    imu1_relative: false
    imu1_queue_size: 10
    imu1_remove_gravitational_acceleration: false

    imu2: /imu2
    imu2_config: [false, false, false,
                false, false, false,
                false, false, false,
                true, true, true,
                false, false, false]
    imu2_nodelay: false
    imu2_differential: true
    imu2_relative: false
    imu2_queue_size: 10
    imu2_remove_gravitational_acceleration: false

    imu3: /imu3
    imu3_config: [false, false, false,
                false, false, false,
                false, false, false,
                true, true, true,
                false, false, false]
    imu3_nodelay: false
    imu3_differential: true
    imu3_relative: false
    imu3_queue_size: 10
    imu3_remove_gravitational_acceleration: false
    use_control: false

    process_noise_covariance: ...

    initial_estimate_covariance: ...

ekf_filter_node_map:
  ros__parameters:

    use_sim_time: true

    frequency: 30.0
    sensor_timeout: 0.1
    two_d_mode: false
    transform_time_offset: 0.0
    transform_timeout: 0.0
    print_diagnostics: false
    debug: false

    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: map
    odom0: /wheel/odom
    odom0_config: [false, false, false,
                  false, false, false,
                  true,  true,  true,
                  false, false, true,
                  false, false, false]
    odom0_queue_size: 10
    odom0_nodelay: true
    odom0_differential: false
    odom0_relative: true

    odom1: /visual_slam//odometry
    odom1_config: [true, true, true,
                  true, true, true,
                  false,  false,  false,
                  false, false, false,
                  false, false, false]
    odom1_queue_size: 10
    odom1_nodelay: true
    odom1_differential: true
    odom1_relative: false

    odom2: /lidar/odom
    odom2_config: [true, true, true,
                  false, false, false,
                  false,  false,  false,
                  false, false, false,
                  false, false, false]
    odom2_queue_size: 10
    odom2_nodelay: true
    odom2_differential: false
    odom2_relative: false

    odom3: /odometry/gps
    odom3_config: [true,  true,  false,
                  false, false, false,
                  false, false, false,
                  false, false, false,
                  false, false, false]
    odom3_queue_size: 10
    odom3_nodelay: true
    odom3_differential: false
    odom3_relative: false

    imu0: /imu/with/absolute/orientation/returns 0 when facing East
    imu0_config: [false, false, false,
                true, true, true,
                false, false, false,
                true, true, true,
                false, false, false]
    imu0_nodelay: false
    imu0_differential: false
    imu0_relative: true # IS THIS CORRECT? IF IT IS FALSE, THE DIRECTION OF base_link ...

(more)

answered 2022-07-28 09:34:33 -0500

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

13689 ●74 ●240 ●232 http://github.com/ayrton04

updated 2022-08-26 03:26:34 -0500

The only issue is that the direction of the GPS odometry seems to be rotated with respect to the other odometry

This says that you have your yaw_offset or magnetic_declination parameters set incorrectly. It could also be a timing thing, especially with the way you have your filter set up. More on that later.

First, you are fusing absolute pose data from four different sources:

/visual_slam//odometry: position and orientation
/lidar/odom: position
/imu/with/absolute/orientation: orientation
/odometry/gps: position

(Your visual slam topic has two slashes in it, by the way).

Anyway, I sincerely doubt these sources are going to agree with one another over time, and so over time, the pose is going to start jumping between them, even without the GPS data. You should choose one as your most trustworthy pose source, and fuse velocity data from the others, or put them in differential mode if they don't provide velocity.

Just so we're clear, navsat_transform_node requires your IMU to read 0 facing east. The EKF doesn't care. navsat_transform_node needs three things:

An IMU with an earth-referenced heading
A GPS fix
Your robot's current pose in its world frame

It doesn't care what (3) is, so long as it's right-handed. It doesn't have to agree at all with the GPS or IMU that you use in n_t_n. You could just use raw wheel odometry alone for your EKF state estimate, and it would work.

But you need to be careful here. If this hapens:

EKF kicks off, gets a single IMU measurement from, e.g., imu3. So its first output has a pose and position of 0, with whatever angular velocity was in that message.
navsat_transform_node is ready, and gets all its input data, including that first pose from the EKF (i.e., input 3 above). That pose is 0.
EKF fuses pose from some other source. Let's say it's from your SLAM system, that says you are at some non-zero position and with non-zero orientation. Now you've effectively changed your EKF's coordinate frame (i.e., where its origin is). But navsat_transform_node has the wrong EKF pose, so when it transforms the GPS data, it will be wrong.

What you want is:

EKF fuses data from all pose sources (again, you preferably one have one source for absolute pose)
navsat_transform_node` is ready, and gets all its input data, including that first pose from the EKF
Now all posese from n_t_n should be consistent with your robot's world frame.

This is why n_t_n has the delay parameter. It gives you time to get a sensor measurement from all your sensors into the EKF before it starts.

Update in response to comments:

should I then set lidar odom to differential=true and gps as absolute x y source?

Up to you, really, but as you have it now, your pose will eventually diverge from the GPS path (lidar odometry will be integrating laser scan matches, I presume ... (more)

edit flag offensive delete link

Comments

"Anyway, I sincerely doubt these sources are going to agree with one another over time, and so over time, the pose is going to start jumping between them, even without the GPS data. You should choose one as your most trustworthy pose source, and fuse velocity data from the others, or put them in differential mode if they don't provide velocity."

in my config, there is only 1 absolute source (most trustworthy lidar odom), i've set the others to differential=true or only fuse the velocities (except imu as i thought n_t_n requires absolute IMU orientation), except in the global odom with lidar and gps odom providing absolute positions. should I then set lidar odom to differential=true and gps as absolute x y source?

requires your IMU to read 0 facing east

Yes it is 0 when facing east, hence yaw_offset = 0

But you need to be ...

(more)

MrOCW ( 2022-07-28 09:47:49 -0500 )edit

add a comment

Fusing GPS in robot_localization

1 Answer

Comments

Question Tools

Stats

Related questions

Fusing GPS in robot_localization edit

1 Answer

Comments

Question Tools

Stats

Related questions

Fusing GPS in robot_localization