Ask Your Question

robot_pose_ekf with mocap

asked 2013-05-26 16:24:31 -0500

I am trying to use the robot_pose_ekf in combination with an IMU and mocap. The mocap data comes in a geometry_msgs/pose but the ekf uses nav_mgs/Odometry. Can I repackage the data from the mocap node using finite differences to get twist? Has anyone else using the robot_pose_ekf with a mocap system?

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted

answered 2013-05-28 08:06:56 -0500

zumili gravatar image

Hello Balto,

First off all, yes you can repack it, but as you can see in nav_msgs/Odometry you also need the covariance matrix of the position/orientation (float64[36] covariance). This includes (x|y|z|roll|pitch|yaw) * (x|y|z|roll|pitch|yaw)^T.

Copied from message description: Row-major representation of the 6x6 covariance matrix. The orientation parameters use a fixed-axis representation. In order, the parameters are: (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)

You can directly fill the pose part of the nav_msgs/Odometry message with your geometry_msgs/Pose message. Now you have to add your covariances into the pose-covariance-matrix, this covariances normally grows during time. So every time you get a new message from mocap you must also modify your covariances. If every covariance in the matrix is zero, the filter thinks that your odometry doesn't produce any error and no correction/filtering is done at this momement because the position at this moment will be 100% correct for the filter and it didn't have to correct it!

The twist part of the nav_msgs/Odometry message can be empty because as you can see in the source code of the robot_pose_ekf (odom_estimation_node.cpp lines 164-205) the robot_pose_ekf only uses position x,y the orientation and of course the covariance matrix of pose. Also the covariance matrix of Twist can be empty for this filter.

Best regards, Thomas

edit flag offensive delete link more



Hi Thomas, Thank you for your reply. The mocap has sub-millimetre accuracy but only updates as 100 Hz but the IMU updates 250 Hz. I wanted the IMU to maximize the update rate. Do you know of any ways to update the covariance matrix of the mocap pose to include the accumulating error?

tonybaltovski gravatar image tonybaltovski  ( 2013-05-28 08:12:43 -0500 )edit

Do you have any recommendation on how to use the filter to get the best rusults? I am using an IMU and an OptiTrack mocap.

Thank you for your help again. Regards, Tony

tonybaltovski gravatar image tonybaltovski  ( 2013-05-28 08:24:05 -0500 )edit

answered 2013-05-30 06:03:55 -0500

mjcarroll gravatar image

I believe that Clearpath Robotics has a modification to robot_pose_ekf in their Github account (, which allows for using absolute positioning systems.

They specifically mention using it in conjunction with a motion capture system (which makes sense, as they also maintain mocap_optitrack.

edit flag offensive delete link more


Thank you so much!

tonybaltovski gravatar image tonybaltovski  ( 2013-05-30 08:06:26 -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

1 follower


Asked: 2013-05-26 16:24:31 -0500

Seen: 712 times

Last updated: May 30 '13