How to calculate the covariance of pose in nav_msgs/Odometry ?

asked 2021-06-10 06:12:50 -0500

KNTRL gravatar image

updated 2022-02-25 03:39:07 -0500

Tom Moore gravatar image

So I am generating data from a 4wheeler with Ackermann steering and calculated the pose and the twist. I think that I also understood the core concept of covariance and variance. What I didn't understand though is how to generate the data myself, or how to calculate it to be exact.

After driving 100m, the field where I am probably located due to odometry is smaller than after driving 1km.

My approach currently is to take all of the saved pose.position/orientation.x/y/z values from the vehicle in the live-simulation and generate a covariance like

pose_cov = np.cov(np.array([self.pos_x, self.pos_y, self.pos_z, self.ori_x, self.ori_y, self.ori_z]))

I am basically doing the same for the twist too, and when looking at rviz the results seem to make sense, since the covariance is increasing with time. However though, I am still not sure if this is the way that I should calculate it. My aim is to feed this to the robot_localization, and I am not sure yet how it will affect the result. I am trying to understand the process step by step and not get confused later on when already working with R_L.

Here it is recommended to use a fixed covariance, but in the same comment chain it is said too that the covariance is increasing out of bounds in reality. So my question is if and how this would affect the R_L later on.

edit retag flag offensive close merge delete