Revision history [back]

Hi! Position covariance from RosAria would not be of much use. Having a simple kinematic model and Gaussian noise in transversal and rotational velocities (zero mean with some covariance), doing only covariance propagation (without correction from external measurements) in order to estimate pose covariance would result in pose covariance growing without bounds. Hence, EKF.

Therefore, robot_pose_ekf only needs covariance estimates for transversal and rotational velocities. Note that robot_pose_ekf doesn't do localization (no external measurements), and also doesn't publish pose covariance.

To estimate velocity covariance you should know TICKSMM (128) and SIPCYCLE (100) parameters of your robot (written in your robots flash memory and not accessible with Aria). First parameter tells you how many encoder impulses (count) gets generated by your robot's forward movement of 1 mm. Second parameter tells you number of milliseconds between two consecutive Server Information Packets from your robot. The values in the parentheses are for P3-DX (ARCOS).

So an error in determining velocity could come from missing an encoder impulse in a cycle. This would result in 1/TICKSMM/SIPCYCLE velocity error (mm/ms or m/s) for one wheel. For P3-DX parameters above, this value is 7.8125e-05. Note that you would err by the same absolute amount of velocity in the next cycle. Gearbox also plays a role in velocity error, but you would need to measure to find the exact amount. As a rule of thumb, I would at least double the previous amount in order to include gearbox error.

Now that we have determined maximum error of a single wheel's (transversal) velocity, i.e. we expect 99.7% of errors to be less than this number, we can determine sigma = max_err/3 and C = sigma^2. Translational and rotational velocities are determined from left and right wheel velocities like this:

v = (v_R + v_L)/2

w = (v_R - v_L)/(2d)

So the covariance for transversal velocity would be (1/2)^2 2C and the covariance for rotational velocity would be (1/(2d))^2 2C. The d parameter is 1/DiffConvFactor and is accessible from Aria (ArRobot::getDiffConvFactor()).

You can always set the covariance matrix to the identity matrix to get things going, but depending on the other sensors covariance, EKF then might "ignore" robot's odometry measurements.