| ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
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!
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.
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?
+1 any reference to the velocity covariance estimation formula?
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:
.png "Alireza"){kind=avatar}
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);
Asked: 2012-01-15 05:49:08 -0500
Seen: 12,808 times
Last updated: Jan 17 '12
ROS Answers is licensed under Creative Commons Attribution 3.0 Content on this site is licensed under a Creative Commons Attribution Share Alike 3.0 license.
.png "Alireza"){kind=avatar}
