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

asked 2019-03-12 00:37:12 -0600

levin
13 ●1 ●2 ●4

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

Are we supposed to post process the messages from the two ekf nodes, and fuse them somehow?
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.

edit retag flag offensive close merge delete

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2019-03-19 06:47:15 -0600

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

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

updated 2019-03-19 06:47:34 -0600

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.

edit flag offensive delete link

Comments

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.

levin ( 2019-04-16 02:12:21 -0600 )edit

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?

i-deterministic ( 2022-05-19 06:57:43 -0600 )edit

Please ask a new question @i-deterministic

Tom Moore ( 2022-06-06 04:13:45 -0600 )edit

add a comment

answered 2019-04-16 02:23:08 -0600

levin
13 ●1 ●2 ●4

updated 2019-04-16 02:26:08 -0600

Lately I've been using robot_localization package to fuse IMU, GPS, and wheel odometry, and below is my current understanding concerning two EKFs.

Why do we need two EKFs (in the context of fusing IMU, GPS, and wheel odometry)

The short answer is that using one EKF in such case is against REP 105.

We want a map EKF so that we can report robot pose in map frame, in the form of published topic and transform. The published topic is odometry/filtered (nav_msgs/Odometry), it contains the estimated map->base_link transform information. If we directly publish the map->base_link transform, we will be against REP 105. It specifies that only map->odom, and odom->base_link transform publish is acceptable. To adhere to REP 105, we use odom ekf to publish odom->base_link transform, and map ekf to publish map->odom transform.

How do two EKFs interact with one another

This has been well addressed in Tom's above answer. Just to summarize:

1) The odom EKF fuses IMU , wheel odometry. It generates odom->base_link transform.
2) The map EKF fuses IMU, wheel odometry, and GPS. It utilizes the odom->base_link transform generated by odom EKF in two areas:
A. Calculate the map->odom transform
B. Converts sensor measurements of odom frame (like those from wheel odometry) to map frame in order to have them fused.

Interpretation of map frame and odom frame

Odom frame is usually decided by sensor source. For example, a wheel odometry may set the starting pose as odom frame. Per the "odom Frame Consistency" section in REP 105, odom frame is subject to change, in the sense that we would need to reset the odom frame when "the integrated position error accumulates enough to make the data invalid". In other words, we would probably need to reset the odom frame of wheel odometry after running for some time, if we are to leverage the position information in the sensor message.

map frame is designated by us, and can be anywhere in the map.

map EKF calculates the relative transform estimate between map and base_link, and odom EKF calculates the relative transform estimate between base_link and odom. If we fix map frame, we can track down base_link, and then we can further track down odom frame. As both EKFs keep updating their estimates, the map->odom transform is not fixed and keeps updating too.

edit flag offensive delete link

add a comment

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

2 Answers

Comments

Question Tools

Stats

Related questions

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

2 Answers

Comments

Question Tools

Stats

Related questions

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