Ask Your Question
3

setting covariances for robot_localization

asked 2015-11-11 10:59:35 -0500

Naman gravatar image

updated 2015-11-13 10:10:38 -0500

Hi all,

I have a mobile robot and I am using robot_localization to fuse data from wheel_encoders, IMU and hokuyo lidar (using AMCL). In the first instance of robot_localization, I have wheel_encoders and IMU in the odom_frame and in the second instance of robot_localization, I have wheel_encoders and output of AMCL (amcl_pose (geometry_msgs/PoseWithCovarianceStamped)) in the map_frame.

This is my launch file for the second instance of robot_localization :

<launch>

<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization2" clear_params="true">

  <param name="frequency" value="10"/>

  <param name="sensor_timeout" value="0.5"/>

  <param name="two_d_mode" value="true"/>

  <param name="map_frame" value="map"/>
  <param name="odom_frame" value="odom_combined"/>
  <param name="base_link_frame" value="base_footprint"/>
  <param name="world_frame" value="map"/>

  <param name="transform_time_offset" value="0.0"/>

  <param name="odom0" value="odom"/>
  <param name="pose0" value="amcl_pose"/>

  <rosparam param="odom0_config">[false, false, false,
                                  false, false, false,
                                  true, true, false,
                                  false, false, true,
                                  false, false, false]</rosparam>

  <rosparam param="pose0_config">[true,  true,  false,
                                  false, false, true,
                                  false, false, false,
                                  false, false, false,
                                  false, false, false]</rosparam>

  <param name="odom0_differential" value="true"/>
  <param name="pose0_differential" value="false"/>

  <param name="print_diagnostics" value="true"/>

  <param name="debug"           value="false"/>
  <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

  <rosparam param="process_noise_covariance">[0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]</rosparam>

       <rosparam param="initial_estimate_covariance">[0.3, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0.3, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0 ...
(more)
edit retag flag offensive close merge delete

Comments

Can you post sample messages? Also, your initial_estimate_covariance should not have values of 1e+9 for variables that you are not measuring. You want it to have large values for variables that you are measuring.

Tom Moore gravatar imageTom Moore ( 2015-11-11 15:04:03 -0500 )edit

I have updated the original question. Thanks!

Naman gravatar imageNaman ( 2015-11-11 21:50:26 -0500 )edit

Can you please plot each of your input and output pose variables independently (please convert the quaternion to Euler angles) against each other? I'd like to see the time delay between input and output.

Tom Moore gravatar imageTom Moore ( 2015-11-12 16:00:25 -0500 )edit

@Tom Moore, I have updated the original question and put the plots. Let me know if you need more information from me.. Thanks again!

Naman gravatar imageNaman ( 2015-11-12 18:31:59 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
4

answered 2015-11-12 07:44:06 -0500

Tom Moore gravatar image

updated 2015-11-19 07:36:20 -0500

For any given measurement, there are two covariance matrices at play:

  1. The state's covariance matrix
  2. The measurement's covariance matrix

The one that concerns me in your case is your state covariance. You can set the initial value for this matrix using the initial_estimate_covariance parameter. Here's the rule you should follow: if you are measuring a variable, make the diagonal value in initial_estimate_covariance larger than that measurement's covariance. So, for example, if your measurement's covariace value for the variable in question is 1e-6, make the initial_estimate_covariance diagonal value 1e-3 or something. This will speed up convergence. If you are not measuring a given variable (e.g., roll), make the diagonal value in initial_estimate_covariance small (but never zero).

EDIT 1

It's hard to tell with the time scale what the lag is between the amcl yaw and the robot_localization yaw. For your plots, you might try (a) zooming in a bit more, and (b) subtracting the first time stamp from one of the data sources from all the timestamps from all sources so that we can see the time in the X axis better.

Also, can you turn OFF the yaw velocity from the wheel encoders in your second r_l config? I want to see the delay when you only have one sensor input.

Also, try increasing the process_noise_covariance for yaw. It will make the error in the state estimate grow faster, which will in turn cause it to trust your measurements more.

Also also, you might want to try the transform_time_offset parameter. I believe amcl future-dates its map->odom transform (with a default value of 0.1 seconds), so it may be worth doing the same.

EDIT 2

I'm not convinced you don't have another problem with transforms. If you are just fusing the pose data from amcl, then the two plots should be much closer to one another. In what frame is the amcl data published? Is amcl also publishing the map->odom_combined transform?

edit flag offensive delete link more

Comments

I have updated the plots and the question as you mentioned. Let me know if there is a need to make further changes to the plots! Thanks a lot!

Naman gravatar imageNaman ( 2015-11-13 10:13:05 -0500 )edit

@Tom Moore, when study r_l,i have a similar question want to ask for your help,if you have time ,please look at this question,i will be very very appreciated,Thanks a lot!

jxl gravatar imagejxl ( 2016-12-30 03:49:23 -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

5 followers

Stats

Asked: 2015-11-11 10:59:35 -0500

Seen: 2,847 times

Last updated: Nov 19 '15