Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

setting covariances for robot_localization

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_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">[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,     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,     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,    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,    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,    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,    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,    0,    0,    0,     0,     0,     0,    0,    1e+9]</rosparam>


  <!--  Placeholder for output topic remapping -->
  <remap from="odometry/filtered" to="odometry/filtered2"/>

</node>

</launch>

The covariance for wheel_encoders is 1e-7, the covariance for IMU is 1e-7 and the covariance for AMCL is its default value which is 0.25 (for initial_covariance in X,Y,Z)

Now, I have following doubts:
1. When the robot starts, it takes lot of time to converge and during that time, the robot weaves a lot.
2. When the robot makes a turn, its localization gets messed up and then it again takes sometime to converge and correct the localization and again, during that time the robot weaves a lot. This happens only when the robot turns.

Also, if I just use one instance of robot_localization with wheel_encoders and IMU in the odom_frame and then use amcl to provide the map->odom transformation, then the robot performs better and weaving of the robot is less and it converges faster. Therefore, I suspect it has something to do with the second instance of robot_localization.

Any help regarding this will be appreciated. Let me know if you need more information from my side.

Thanks in advance.
Naman Kumar

setting covariances for robot_localization

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_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">[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,     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,     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,    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,    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,    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,    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,    0,    0,    0,     0,     0,     0,    0,    1e+9]</rosparam>


  <!--  Placeholder for output topic remapping -->
  <remap from="odometry/filtered" to="odometry/filtered2"/>

</node>

</launch>

The covariance for wheel_encoders is 1e-7, the covariance for IMU is 1e-7 and the covariance for AMCL is its default value which is 0.25 (for initial_covariance in X,Y,Z)

Now, I have following doubts:
1. When the robot starts, it takes lot of time to converge and during that time, the robot weaves a lot.
2. When the robot makes a turn, its localization gets messed up and then it again takes sometime to converge and correct the localization and again, during that time the robot weaves a lot. This happens only when the robot turns.

Also, if I just use one instance of robot_localization with wheel_encoders and IMU in the odom_frame and then use amcl to provide the map->odom transformation, then the robot performs better and weaving of the robot is less and it converges faster. Therefore, I suspect it has something to do with the second instance of robot_localization.

Any help regarding this will be appreciated. Let me know if you need more information from my side.

Update 1 :

Sample odometry (/odom) message :

header: 
  seq: 1373
  stamp: 
    secs: 1446740943
    nsecs: 414337267
  frame_id: odom_combined
child_frame_id: base_footprint
pose: 
  pose: 
    position: 
      x: 1.55659262525
      y: -0.219513382396
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.215424694594
      w: 0.976520455986
  covariance: [1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-05]
twist: 
  twist: 
    linear: 
      x: 0.25119998306
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: -0.279628895983
  covariance: [1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07]

Sample amcl (/amcl_pose) message :

header: 
  seq: 166
  stamp: 
    secs: 1446741053
    nsecs: 58572500
  frame_id: map
pose: 
  pose: 
    position: 
      x: 4.81614080312
      y: 2.14873797135
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.109653845072
      w: 0.99396983569
  covariance: [0.017594585241578642, -0.000982540070030069, 0.0, 0.0, 0.0, 0.0, -0.000982540070030069, 0.008169362154903936, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.009693177766675111]

Sample imu (/imu_data) message :

header: 
  seq: 3312
  stamp: 
    secs: 1446741137
    nsecs: 281814968
  frame_id: imu_frame
orientation: 
  x: 0.00522551208583
  y: 0.013448674883
  z: -0.106760334929
  w: -0.992483813037
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 0.00222162960486
  y: 0.000351509311352
  z: -0.131620340348
angular_velocity_covariance: [1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07]
linear_acceleration: 
  x: 0.317059213562
  y: -0.175599007511
  z: 9.73220711629
linear_acceleration_covariance: [8.66124974095918e-06, 0.0, 0.0, 0.0, 8.66124974095918e-06, 0.0, 0.0, 0.0, 8.66124974095918e-06]

Also for second instance of robot_localization, I am using x,y,yaw (from amcl) and vx,vy,vyaw (encoders) and I have set the covariances for them but left covariances as 1e+9 for variables which I am not measuring. What should be the covariance values for variables which I am not using, should I make it 0? In robot_pose_ekf, I just increased the covariance values to a very large number for variables which I am not measuring.

Thanks in advance.
Naman Kumar

setting covariances for robot_localization

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">[1e-9, 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,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    1e+9, 0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    1e+9, 0,    0,    0.1, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    1e-3, 0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    1e-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,    0,    0,    0,    1e-9, 0,    0,    0,     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,     1e-3 ,  0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    1e+9,  0,     0,     0,    0,     0,     0,     1e-9, 0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e+9,  0,     0,    0,    0,     0,     0,    1e-9, 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,    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]</rosparam>


  <!--  Placeholder for output topic remapping -->
0,     0,    0,    1e-9]</rosparam>


  <remap from="odometry/filtered" to="odometry/filtered2"/>

</node>

</launch>

The covariance for wheel_encoders is 1e-7, the covariance for IMU is 1e-7 and the initial covariance for AMCL is its default value which is 0.25 (for initial_covariance in X,Y,Z)

Now, I have following doubts:
1. When the robot starts, it takes lot of time to converge and during that time, the robot weaves a lot.
2. When the robot makes a turn, its localization gets messed up and then it again takes sometime to converge and correct the localization and again, during that time the robot weaves a lot. This happens only when the robot turns.

Also, if I just use one instance of robot_localization with wheel_encoders and IMU in the odom_frame and then use amcl to provide the map->odom transformation, then the robot performs better and weaving of the robot is less and it converges faster. Therefore, I suspect it has something to do with the second instance of robot_localization.

Any help regarding this will be appreciated. Let me know if you need more information from my side.

Update 1 :

Sample odometry (/odom) message :

header: 
  seq: 1373
  stamp: 
    secs: 1446740943
    nsecs: 414337267
  frame_id: odom_combined
child_frame_id: base_footprint
pose: 
  pose: 
    position: 
      x: 1.55659262525
      y: -0.219513382396
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.215424694594
      w: 0.976520455986
  covariance: [1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-05]
twist: 
  twist: 
    linear: 
      x: 0.25119998306
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: -0.279628895983
  covariance: [1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07]

Sample amcl (/amcl_pose) message :

header: 
  seq: 166
  stamp: 
    secs: 1446741053
    nsecs: 58572500
  frame_id: map
pose: 
  pose: 
    position: 
      x: 4.81614080312
      y: 2.14873797135
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.109653845072
      w: 0.99396983569
  covariance: [0.017594585241578642, -0.000982540070030069, 0.0, 0.0, 0.0, 0.0, -0.000982540070030069, 0.008169362154903936, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.009693177766675111]

Sample imu (/imu_data) message :

header: 
  seq: 3312
  stamp: 
    secs: 1446741137
    nsecs: 281814968
  frame_id: imu_frame
orientation: 
  x: 0.00522551208583
  y: 0.013448674883
  z: -0.106760334929
  w: -0.992483813037
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 0.00222162960486
  y: 0.000351509311352
  z: -0.131620340348
angular_velocity_covariance: [1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07]
linear_acceleration: 
  x: 0.317059213562
  y: -0.175599007511
  z: 9.73220711629
linear_acceleration_covariance: [8.66124974095918e-06, 0.0, 0.0, 0.0, 8.66124974095918e-06, 0.0, 0.0, 0.0, 8.66124974095918e-06]

Also for second instance of robot_localization, I am using x,y,yaw (from amcl) and vx,vy,vyaw (encoders) and I have set the covariances for them but left covariances as 1e+9 for variables which I am not measuring. What should be the covariance values for variables which I am not using, should I make it 0? In robot_pose_ekf, I just increased the covariance values to a very large number for variables which I am not measuring.

Update 2:
Thanks @Tom Moore for the answer. I have updated the launch file (see above) but the robot still behaves the same way as mentioned above, restating here:
1. When the robot starts, it takes lot of time to converge and during that time, the robot weaves a lot.
2. When the robot makes a turn, it overshoots the path and then tries to come back on the path and overshoots on the other side and so on and then it again takes sometime to converge and come to the path but although the weaving reduces, it is still there. This happens only when the robot turns. When the robot goes straight on its path, it still weaves but turning causes lot of weaving.

Some more information about the robot : Its dimension is 92 cm * 64 cm * 100 cm. Its center of rotation is 10 cm from back of the robot and that is where I have specified the base_link of the robot. I am using move_base for planning with sbpl_lattice_planner as a global planner and dwa_local_planner as a local_planner.

This is the link to the video. Green arrows represent the output of second instance of robot_localization, blue arrows represent the output of first instance of robot_localization and red arrow is the odometry from the wheel_encoders.

Let me know if you need more information from me.

Thanks in advance.
Naman Kumar

setting covariances for robot_localization

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,     0,    0,    0,
                                                      0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0.1, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    1e-3, 0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    1e-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,    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-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,    0,    0,    0,     0,     0,     0,    0,    1e-9]</rosparam>


  <remap from="odometry/filtered" to="odometry/filtered2"/>

</node>

</launch>

The covariance for wheel_encoders is 1e-7, the covariance for IMU is 1e-7 and the initial covariance for AMCL is its default value which is 0.25 (for initial_covariance in X,Y,Z)

Now, I have following doubts:
1. When the robot starts, it takes lot of time to converge and during that time, the robot weaves a lot.
2. When the robot makes a turn, its localization gets messed up and then it again takes sometime to converge and correct the localization and again, during that time the robot weaves a lot. This happens only when the robot turns.

Also, if I just use one instance of robot_localization with wheel_encoders and IMU in the odom_frame and then use amcl to provide the map->odom transformation, then the robot performs better and weaving of the robot is less and it converges faster. Therefore, I suspect it has something to do with the second instance of robot_localization.

Any help regarding this will be appreciated. Let me know if you need more information from my side.

Update 1 :

Sample odometry (/odom) message :

header: 
  seq: 1373
  stamp: 
    secs: 1446740943
    nsecs: 414337267
  frame_id: odom_combined
child_frame_id: base_footprint
pose: 
  pose: 
    position: 
      x: 1.55659262525
      y: -0.219513382396
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.215424694594
      w: 0.976520455986
  covariance: [1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-05]
twist: 
  twist: 
    linear: 
      x: 0.25119998306
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: -0.279628895983
  covariance: [1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07]

Sample amcl (/amcl_pose) message :

header: 
  seq: 166
  stamp: 
    secs: 1446741053
    nsecs: 58572500
  frame_id: map
pose: 
  pose: 
    position: 
      x: 4.81614080312
      y: 2.14873797135
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.109653845072
      w: 0.99396983569
  covariance: [0.017594585241578642, -0.000982540070030069, 0.0, 0.0, 0.0, 0.0, -0.000982540070030069, 0.008169362154903936, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.009693177766675111]

Sample imu (/imu_data) message :

header: 
  seq: 3312
  stamp: 
    secs: 1446741137
    nsecs: 281814968
  frame_id: imu_frame
orientation: 
  x: 0.00522551208583
  y: 0.013448674883
  z: -0.106760334929
  w: -0.992483813037
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 0.00222162960486
  y: 0.000351509311352
  z: -0.131620340348
angular_velocity_covariance: [1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07]
linear_acceleration: 
  x: 0.317059213562
  y: -0.175599007511
  z: 9.73220711629
linear_acceleration_covariance: [8.66124974095918e-06, 0.0, 0.0, 0.0, 8.66124974095918e-06, 0.0, 0.0, 0.0, 8.66124974095918e-06]

Also for second instance of robot_localization, I am using x,y,yaw (from amcl) and vx,vy,vyaw (encoders) and I have set the covariances for them but left covariances as 1e+9 for variables which I am not measuring. What should be the covariance values for variables which I am not using, should I make it 0? In robot_pose_ekf, I just increased the covariance values to a very large number for variables which I am not measuring.

Update 2:
Thanks @Tom Moore for the answer. I have updated the launch file (see above) but the robot still behaves the same way as mentioned above, restating here:
1. When the robot starts, it takes lot of time to converge and during that time, the robot weaves a lot.
2. When the robot makes a turn, it overshoots the path and then tries to come back on the path and overshoots on the other side and so on and then it again takes sometime to converge and come to the path but although the weaving reduces, it is still there. This happens only when the robot turns. When the robot goes straight on its path, it still weaves but turning causes lot of weaving.

Some more information about the robot : Its dimension is 92 cm * 64 cm * 100 cm. Its center of rotation is 10 cm from back of the robot and that is where I have specified the base_link of the robot. I am using move_base for planning with sbpl_lattice_planner as a global planner and dwa_local_planner as a local_planner.

This is the link to the video. Green arrows represent the output of second instance of robot_localization, blue arrows represent the output of first instance of robot_localization and red arrow is the odometry from the wheel_encoders.

Update 3:

The three plots are shown below for robot_localization 2nd instance ( Input : wheel_encoders odometry and AMCL and Output : odometry/filtered2). Please note that I am using x,y,yaw from AMCL (differential = false) and vx,vy,vyaw from wheel_encoders for second instance of robot_localization (see launch file above).

Plot-1- (pose/pose/position/X) image description

Plot-2- (pose/pose/position/Y) image description

Plot-3- (pose/pose/orientation --> Yaw) image description

Let me know if you need more information from me.

Thanks in advance.
Naman Kumar

setting covariances for robot_localization

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,     0,    0,    0,
                                                      0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0.1, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    1e-3, 0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    1e-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,    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-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,    0,    0,    0,     0,     0,     0,    0,    1e-9]</rosparam>


  <remap from="odometry/filtered" to="odometry/filtered2"/>

</node>

</launch>

The covariance for wheel_encoders is 1e-7, the covariance for IMU is 1e-7 and the initial covariance for AMCL is its default value which is 0.25 (for initial_covariance in X,Y,Z)

Now, I have following doubts:
1. When the robot starts, it takes lot of time to converge and during that time, the robot weaves a lot.
2. When the robot makes a turn, its localization gets messed up and then it again takes sometime to converge and correct the localization and again, during that time the robot weaves a lot. This happens only when the robot turns.

Also, if I just use one instance of robot_localization with wheel_encoders and IMU in the odom_frame and then use amcl to provide the map->odom transformation, then the robot performs better and weaving of the robot is less and it converges faster. Therefore, I suspect it has something to do with the second instance of robot_localization.

Any help regarding this will be appreciated. Let me know if you need more information from my side.

Update 1 :

Sample odometry (/odom) message :

header: 
  seq: 1373
  stamp: 
    secs: 1446740943
    nsecs: 414337267
  frame_id: odom_combined
child_frame_id: base_footprint
pose: 
  pose: 
    position: 
      x: 1.55659262525
      y: -0.219513382396
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.215424694594
      w: 0.976520455986
  covariance: [1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-05]
twist: 
  twist: 
    linear: 
      x: 0.25119998306
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: -0.279628895983
  covariance: [1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07]

Sample amcl (/amcl_pose) message :

header: 
  seq: 166
  stamp: 
    secs: 1446741053
    nsecs: 58572500
  frame_id: map
pose: 
  pose: 
    position: 
      x: 4.81614080312
      y: 2.14873797135
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.109653845072
      w: 0.99396983569
  covariance: [0.017594585241578642, -0.000982540070030069, 0.0, 0.0, 0.0, 0.0, -0.000982540070030069, 0.008169362154903936, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.009693177766675111]

Sample imu (/imu_data) message :

header: 
  seq: 3312
  stamp: 
    secs: 1446741137
    nsecs: 281814968
  frame_id: imu_frame
orientation: 
  x: 0.00522551208583
  y: 0.013448674883
  z: -0.106760334929
  w: -0.992483813037
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 0.00222162960486
  y: 0.000351509311352
  z: -0.131620340348
angular_velocity_covariance: [1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07]
linear_acceleration: 
  x: 0.317059213562
  y: -0.175599007511
  z: 9.73220711629
linear_acceleration_covariance: [8.66124974095918e-06, 0.0, 0.0, 0.0, 8.66124974095918e-06, 0.0, 0.0, 0.0, 8.66124974095918e-06]

Also for second instance of robot_localization, I am using x,y,yaw (from amcl) and vx,vy,vyaw (encoders) and I have set the covariances for them but left covariances as 1e+9 for variables which I am not measuring. What should be the covariance values for variables which I am not using, should I make it 0? In robot_pose_ekf, I just increased the covariance values to a very large number for variables which I am not measuring.

Update 2:
Thanks @Tom Moore for the answer. I have updated the launch file (see above) but the robot still behaves the same way as mentioned above, restating here:
1. When the robot starts, it takes lot of time to converge and during that time, the robot weaves a lot.
2. When the robot makes a turn, it overshoots the path and then tries to come back on the path and overshoots on the other side and so on and then it again takes sometime to converge and come to the path but although the weaving reduces, it is still there. This happens only when the robot turns. When the robot goes straight on its path, it still weaves but turning causes lot of weaving.

Some more information about the robot : Its dimension is 92 cm * 64 cm * 100 cm. Its center of rotation is 10 cm from back of the robot and that is where I have specified the base_link of the robot. I am using move_base for planning with sbpl_lattice_planner as a global planner and dwa_local_planner as a local_planner.

This is the link to the video. Green arrows represent the output of second instance of robot_localization, blue arrows represent the output of first instance of robot_localization and red arrow is the odometry from the wheel_encoders.

Update 3:

The three plots are shown below for robot_localization 2nd instance ( Input : wheel_encoders odometry and AMCL and Output : odometry/filtered2). Please note that I am using x,y,yaw from AMCL (differential = false) and vx,vy,vyaw from wheel_encoders for second instance of robot_localization (see launch file above).above) (world_frame = map). AMCL is in map frame and wheel_encoders odometry is in odom_combined frame.

Plot-1- (pose/pose/position/X) image description

Plot-2- (pose/pose/position/Y) image description

Plot-3- (pose/pose/orientation --> Yaw) image description

Let me know if you need more information from me.

Thanks in advance.
Naman Kumar

setting covariances for robot_localization

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,     0,    0,    0,
                                                      0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0.1, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    1e-3, 0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    1e-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,    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-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,    0,    0,    0,     0,     0,     0,    0,    1e-9]</rosparam>


  <remap from="odometry/filtered" to="odometry/filtered2"/>

</node>

</launch>

The covariance for wheel_encoders is 1e-7, the covariance for IMU is 1e-7 and the initial covariance for AMCL is its default value which is 0.25 (for initial_covariance in X,Y,Z)

Now, I have following doubts:
1. When the robot starts, it takes lot of time to converge and during that time, the robot weaves a lot.
2. When the robot makes a turn, its localization gets messed up and then it again takes sometime to converge and correct the localization and again, during that time the robot weaves a lot. This happens only when the robot turns.

Also, if I just use one instance of robot_localization with wheel_encoders and IMU in the odom_frame and then use amcl to provide the map->odom transformation, then the robot performs better and weaving of the robot is less and it converges faster. Therefore, I suspect it has something to do with the second instance of robot_localization.

Any help regarding this will be appreciated. Let me know if you need more information from my side.

Update 1 :

Sample odometry (/odom) message :

header: 
  seq: 1373
  stamp: 
    secs: 1446740943
    nsecs: 414337267
  frame_id: odom_combined
child_frame_id: base_footprint
pose: 
  pose: 
    position: 
      x: 1.55659262525
      y: -0.219513382396
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.215424694594
      w: 0.976520455986
  covariance: [1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-05]
twist: 
  twist: 
    linear: 
      x: 0.25119998306
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: -0.279628895983
  covariance: [1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07]

Sample amcl (/amcl_pose) message :

header: 
  seq: 166
  stamp: 
    secs: 1446741053
    nsecs: 58572500
  frame_id: map
pose: 
  pose: 
    position: 
      x: 4.81614080312
      y: 2.14873797135
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.109653845072
      w: 0.99396983569
  covariance: [0.017594585241578642, -0.000982540070030069, 0.0, 0.0, 0.0, 0.0, -0.000982540070030069, 0.008169362154903936, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.009693177766675111]

Sample imu (/imu_data) message :

header: 
  seq: 3312
  stamp: 
    secs: 1446741137
    nsecs: 281814968
  frame_id: imu_frame
orientation: 
  x: 0.00522551208583
  y: 0.013448674883
  z: -0.106760334929
  w: -0.992483813037
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 0.00222162960486
  y: 0.000351509311352
  z: -0.131620340348
angular_velocity_covariance: [1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07]
linear_acceleration: 
  x: 0.317059213562
  y: -0.175599007511
  z: 9.73220711629
linear_acceleration_covariance: [8.66124974095918e-06, 0.0, 0.0, 0.0, 8.66124974095918e-06, 0.0, 0.0, 0.0, 8.66124974095918e-06]

Also for second instance of robot_localization, I am using x,y,yaw (from amcl) and vx,vy,vyaw (encoders) and I have set the covariances for them but left covariances as 1e+9 for variables which I am not measuring. What should be the covariance values for variables which I am not using, should I make it 0? In robot_pose_ekf, I just increased the covariance values to a very large number for variables which I am not measuring.

Update 2:
Thanks @Tom Moore for the answer. I have updated the launch file (see above) but the robot still behaves the same way as mentioned above, restating here:
1. When the robot starts, it takes lot of time to converge and during that time, the robot weaves a lot.
2. When the robot makes a turn, it overshoots the path and then tries to come back on the path and overshoots on the other side and so on and then it again takes sometime to converge and come to the path but although the weaving reduces, it is still there. This happens only when the robot turns. When the robot goes straight on its path, it still weaves but turning causes lot of weaving.

Some more information about the robot : Its dimension is 92 cm * 64 cm * 100 cm. Its center of rotation is 10 cm from back of the robot and that is where I have specified the base_link of the robot. I am using move_base for planning with sbpl_lattice_planner as a global planner and dwa_local_planner as a local_planner.

This is the link to the video. Green arrows represent the output of second instance of robot_localization, blue arrows represent the output of first instance of robot_localization and red arrow is the odometry from the wheel_encoders.

Update 3:3 and Update 4:

The three plots are shown below for robot_localization 2nd instance ( Input : wheel_encoders odometry and AMCL and Output : odometry/filtered2). Please note that I am using x,y,yaw from AMCL (differential = false) and vx,vy,vyaw from wheel_encoders for second instance of robot_localization (see launch file above) (world_frame = map). AMCL is in map frame and wheel_encoders odometry is in odom_combined frame.

Plot-1- (pose/pose/position/X) image descriptionimage description

Plot-2- (pose/pose/position/Y) image descriptionimage description

Plot-3- (pose/pose/orientation --> YawYAW) image descriptionimage description

Now, I have turned off the YAW velocity (vyaw) from encoders in the second instance of robot_localization and this is the video. The performance is really bad.
Note: I am using Ultrawide band sensor to set the initial pose and hence it is not perfect but robot localization is able to correct it

Here are the plots for above settings :

Plot-1- (pose/pose/position/X) image description

Plot-2- (pose/pose/position/Y) image description

Plot-3- (pose/pose/orientation --> YAW) image description

I will run the robot after increasing process_noise_covariance for yaw and also setting transform_time_offset to 0.1 and update the question with the output.

Let me know if you need more information from me.

Thanks in advance.
Naman Kumar