Dynamic Pose Covariance for ndt_pose

edit

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 .

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!

Thanks alot! These are really useful.