Robotics StackExchange | Archived questions

Dynamic Pose Covariance for ndt_pose

I am trying to fuse autoware.AI's ndtpose into the robotlocalization package with other sensor data. However I will need to add covariance to ndtpose to convert the message type from geometrymsgs/PoseStamped to geometry_msgs/PoseWithCovarianceStamped.

Is there a good way to generate the ndt_pose covariance dynamically (perhaps based on the Fitness Score)? I am currently using static covariance but I feel that dynamic covariance will be better for the localisation performance.

Edited

I've decided to calculate simple moving average (SMA) and exponential moving average (EMA) to dynamically derive the covariance. I'm more inclined to use the covariance derived from the exponential moving average and I chose a time period which is close to the rate of publishing of the /ndt_pose topic.

Nonetheless, are there other better ways to dynamically generate the ndt_pose covariance?

SMA and EMA formulas = https://en.wikipedia.org/wiki/Moving_average

Asked by Muhd Hizam on 2021-11-01 23:57:57 UTC

Comments

Answers

Having worked with the NDT localization package of the Autoware.AI for quite a while with various car sized applications and I can say that fitness score is not a reliable source of matched-ness. And it couldn't reliably report its health state.

It could be very low or high in various parts of the point cloud map and it could tell it's doing fine when it is floating all around the map.

I wouldn't attempt to directly calculate the covariance with the current state of the algorithm.

I suggest you use a combination of some wheel odometry, imu, vehicle model and decide that the localization is broken when the pose coming from NDT Matching algorithm disagrees with these sources more than some threshold.

Asked by xmfcx on 2021-11-02 14:52:52 UTC

Comments

Thanks so much! Can I just clarify the following:

1) Is it still a good idea to add a static covariance to the ndt_pose (i'm only doing this to fuse it to the robot localisation package)? If yes, do you have any recommendations on the static covariance values to use?

2) With reference to your last para, do you also imply that I should not add the ndt_pose into the EKF (i.e. to generate the ndt_pose output and ekf_output separately and then compare the pose)?

I understand that the autoware.ai package also provides the ekf_localizer package which can take in ndt_pose without covariance. But I have not tried and read the full details of it yet .

Asked by Muhd Hizam on 2021-11-02 22:16:45 UTC

1) Static covariance means, you assign a constant distrust ratio to a all of the readings. Covariance matrix in pose is a 6x6 matrix. The diagonal (top left to bottom right) can be made from variances in order of x, y, z, r, p, y. Variance is the square of the RMSE values of each reading. So if your robot is small, moving slow, it could be small. If your robot is large and moving fast it could be larger. You could keep a constant covariance, it would be simpler in case of NDT in autoware.ai.

2) If you are planning to add ndt poses to the ekf localizer, you need to fully understand the ekf localizer node and be able to reject non-fit ndt inputs. It's a lot of work though, good luck!

Asked by xmfcx on 2021-11-03 03:12:30 UTC

Thanks alot! These are really useful.

Asked by Muhd Hizam on 2021-11-03 10:52:27 UTC