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

Question

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,

---

Originally posted by HK on ROS Answers with karma: 1 on 2017-03-08

Post score: 0

Answer 1

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.

---

Originally posted by Nicholash Bedi with karma: 68 on 2017-03-08

This answer was ACCEPTED on the original site

Post score: 0

---

Original comments

Comment by HK on 2017-03-08:
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.

Comment by Nicholash Bedi on 2017-03-09:
Do you know if your sensor message covariances are correct? Increasing the covariance may smooth out the output of robot_localization.

Comment by HK on 2017-03-10:
Oh yeah, covariance of EKF might be a key to solve this. I will test on the value. Thanks a lot for your advice.