Ask Your Question
5

calculating covariances of robot's odometry ?

asked 2012-01-15 05:49:08 -0500

Alireza gravatar image

Hi we are using RosAria for our pioneer robot, it has an odometry topic named pose, but the values of its covariances are always zero. how can i calculate covariances for odometry message? Actually i want to give RosAria to robot_pose_ekf for combining robot's odometry with IMU data but robot_pose_ekf does not accepts zero covariances!

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
3

answered 2012-01-17 03:47:43 -0500

jrcapriles gravatar image

Hi, I think if you are running directly the RosAria node without modification (running the code in opt) for your robot, the odometry will be updated but the covariance will be always published as 0. The controller don't include the covariance information.

If you need to have a covariance value included in the odometry information for some reason, You can create your own controller based in the RosAria and add the covariance value. Something like:

tf::poseTFToMsg(tf::Pose(tf::Quaternion(pos.getTh()*M_PI/180, 0, 0),  tf::Vector3(pos.getX()/1000, pos.getY()/1000, 0)), position.pose.pose); //Aria returns pose in mm.
  position.pose.covariance =  boost::assign::list_of(1e-3) (0) (0)  (0)  (0)  (0)
                                                       (0) (1e-3)  (0)  (0)  (0)  (0)
                                                       (0)   (0)  (1e6) (0)  (0)  (0)
                                                       (0)   (0)   (0) (1e6) (0)  (0)
                                                       (0)   (0)   (0)  (0) (1e6) (0)
                                                       (0)   (0)   (0)  (0)  (0)  (1e3) ;

  position.twist.twist.linear.x = robot->getVel()/1000; //Aria returns velocity in mm/s.
  position.twist.twist.angular.z = robot->getRotVel()*M_PI/180;
  position.twist.covariance =  boost::assign::list_of(1e-3) (0)   (0)  (0)  (0)  (0)
                                                      (0) (1e-3)  (0)  (0)  (0)  (0)
                                                      (0)   (0)  (1e6) (0)  (0)  (0)
                                                      (0)   (0)   (0) (1e6) (0)  (0)
                                                      (0)   (0)   (0)  (0) (1e6) (0)
                                                      (0)   (0)   (0)  (0)  (0)  (1e3) ; 


  position.header.frame_id = frame_id_odom;
  position.header.stamp = ros::Time::now();
  pose_pub.publish(position);
edit flag offensive delete link more

Comments

Wow! Dear JKS and JoseCapriles, Thanks for helping me with full details!

Alireza gravatar image Alireza  ( 2012-01-17 04:17:12 -0500 )edit
Keep in mind that technically those covariances for the pose aren't constant - they grow without bound if they are only from odometry (as @JKS mentions).
Eric Perko gravatar image Eric Perko  ( 2012-01-17 07:55:51 -0500 )edit
Yes, yes, as you say, the covariance will grow without bound once you start to running your robot, It was a example of how integrate the covariance information into odom message.
jrcapriles gravatar image jrcapriles  ( 2012-01-18 21:16:47 -0500 )edit

Hey Alireza! are you able to compile or work with the codes mentioned above? I am facing the same problem, whenever I add the extra codes and initiate catkin_make (i'm using ROS Groovy), i'll received error: 'boost::assign' has not been declared

barrybear gravatar image barrybear  ( 2013-06-10 06:00:36 -0500 )edit
12

answered 2012-01-17 02:53:35 -0500

JKS gravatar image

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.

edit flag offensive delete link more

Comments

How do you get the wheel velocities from RosAria?

Sven Kratz gravatar image Sven Kratz  ( 2013-05-01 09:18:22 -0500 )edit

I'm having the same trouble about estimating the covariance values for the velocity reading sensor. Can you give any reference to the velocity covariance estimation formula?

sozgundemir gravatar image sozgundemir  ( 2017-03-13 06:06:47 -0500 )edit

+1 any reference to the velocity covariance estimation formula?

danielsnider gravatar image danielsnider  ( 2017-04-12 22:09:35 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

4 followers

Stats

Asked: 2012-01-15 05:49:08 -0500

Seen: 8,473 times

Last updated: Jan 17 '12