robot_localization dual-EKF:How the two ekf nodes work together

Question

I've been puzzled by the concept of dual EKF for days, and really appreciate it if someone could kindly help me out. I checked this post about dual ekf, but I am still confused. Thanks.

In robot localization package's wiki section Integrating GPS Data, we have

If you want to fuse data from a GPS into your position estimate, one potential solution is to do the following:

Run one instance of a robot_localization state estimation node that fuses only continuous data, such as odometry and IMU data. Set the world_frame parameter for this instance to the same value as the odom_frame parameter. Execute local path plans and motions in this frame.

Run another instance of a robot_localization state estimation node that fuses all sources of data, including the GPS. Set the world_frame parameter for this instance to the same value as the map_frame parameter

In the context of using robot location package to fuse IMU and GPS, my understanding is that the wiki recommends us to use two ekf nodes. the first one fuses only IMU, and the second ekf fuses GPS.

At the end of the day, the localization module is supposed to continuously output state estimation via the ekf_localization_node 's published Topics (odometry/filtered and accel/filtered), so the two ekf nodes will publish their own separate messages via these topics. My bewilderment is what happened to the "fusion". Specifically

1. Are we supposed to post process the messages from the two ekf nodes, and fuse them somehow?
2. Or maybe there is already some interactions occurred between the two ekf nodes, and the messages they output are already the fused result, except the first one report the result in odom frame, and the second one report the result in map frame? If this is the case, how the two ekf nodes interact with each each, and get IMU and GPS data fused?

A note that might be of importance, currently my understanding is that both odom frame and map frame are world fixed frame, and the transform between them are fixed once defined. The odom frame refers to the starting position of the robot, while map frame refers to the origin of the map.

---

Originally posted by levin on ROS Answers with karma: 13 on 2019-03-12

Post score: 1

Answer 1

A note that might be of importance, currently my understanding is that both odom frame and map frame are world fixed frame, and the transform between them are fixed once defined. The odom frame refers to the starting position of the robot, while map frame refers to the origin of the map.

I think this is the root of your confusion. The map->odom transform is not static. I'm assuming you've read REP-105. I'll give you an example.

Let's say you have three data sources: wheel odometry, IMU data, and GPS. You have two EKFs. The first one fuses just wheel odometry and IMU data. The second fuses wheel odometry, IMU, and GPS data. Now imagine you take your robot outside and drive it in a giant figure 8, returning exactly to where you started.

If you visualize the output of the first EKF instance, it will be smooth and continuous. However, its final pose won't line up with the starting pose in your visualization, due to drift.

If you visualize the output of the second EKF instance, it will be very jagged, as it will have lots of discrete discontinuities ("jumps"). However, your last pose is likely to line up very well with the first pose, because you aren't subject to drift.

This is what the EKFs are doing. The odom frame EKF fuses only continuous data that is useful for, e.g., local planning. The map frame EKF fuses all data, including the discontinuous GPS data. This is useful if you need a globally accurate position for your robot to, e.g., generate a global plan.

The two EKFs have no interaction whatsoever, except that the map frame EKF looks up the odom->base_link transform generated by the first EKF, so that it can produce the map->odom transform (since it's not allowed to produce map->base_link).

Final note: if all you have is an IMU and GPS, then (a) this isn't very well supported by r_l, and (b) there's no reason for you to use two EKFs. Just fuse the IMU and GPS data into a single state estimate and be done with it. You should really have a velocity reference if you want decent results, though.

---

Originally posted by Tom Moore with karma: 13689 on 2019-03-19

This answer was ACCEPTED on the original site

Post score: 5

---

Original comments

Comment by levin on 2019-04-16:
Hi Tom, I sincerely appreciate your reply, which helped me a lot. Thank you!

It looks that long comment is not allowed in this forum. so I started another answer. Please kindly let me know if you have any comments/advice.

Comment by i-deterministic on 2022-05-19:
Hi Tom, Can you pls tell me how to generate odom to map frame to be fed into the second node of ekf ?. Ultimately I have to combine both results so that I have both locally and globally accurate path of robot, so any comments on that?

Comment by Tom Moore on 2022-06-06:
Please ask a new question @i-deterministic