Ask Your Question

How to exclude poses of a wheel based robot which place behind of the porevious pose

asked 2017-03-08 11:48:04 -0600

HK gravatar image

This question might not be regaring ROS itself but I need your help on below issue regarding robot pose by sensor fusion with EKF.

I am currently working on coding sensor fusion of a wheel based robot pose from GPS, Lidar, Vision and Vehicle measure. Its model is basic kinematics using EKF and no discrimination against sensors i.e. data comes in based on time stamp.

I have difficulty to fuse those sensors due to following issue; Sometimes when the latest incoming data comes in from different sensor from a sensor gave previous state, the latest pose of the robot comes in behind previous pose. Therefore data fusion does not get so smooth and zigzag-ed as a result.

I would like discard data which plots behind/backwards of the previous data and take data which poses always forward/ahead of previous state even when sensor to provide the data changes between timestamp t and timestamp t+1. Since the data frame is global frame, it is impossible to rely on its x coordinate in minus to achieve this.

Please let me know if you had some idea on this. Thank you so much in advance. Best,

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2017-03-08 15:30:24 -0600

Nicholash Bedi gravatar image

To get robot_localization to take the time of the incoming messages into consideration, set smooth_lagged_data to true. You will also need to set history_length appropriately for your situation.

edit flag offensive delete link more


Actually data is not lagged but some sensor pose is behind another sensor's pose when neighboring time stamps are fairly close. So I guess this is due to sensor noise. I am not too sure if history length can deal with this problem. Thank you for your advise, Nicholash.

HK gravatar imageHK ( 2017-03-08 21:03:29 -0600 )edit

Do you know if your sensor message covariances are correct? Increasing the covariance may smooth out the output of robot_localization.

Nicholash Bedi gravatar imageNicholash Bedi ( 2017-03-09 08:59:57 -0600 )edit

Oh yeah, covariance of EKF might be a key to solve this. I will test on the value. Thanks a lot for your advice.

HK gravatar imageHK ( 2017-03-09 23:51:55 -0600 )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

1 follower


Asked: 2017-03-08 11:48:04 -0600

Seen: 39 times

Last updated: Mar 08 '17