Problems with ekf + amcl: particles cloud diverges

edit

Hello

I'm trying to use the navigation stack with the odometry given by an EKF that fuses three sensors: the encoder based odometry, an IMU, and a GPS. The output of the EKF seems to be nice, and if it's relevant publishes the position in UTM's.

The problems start when I use the AMCL for doing the localization on the map. When I run the AMCL, the particles cloud begins to diverge, and then the localization is very poor and the move_base package can't make the robot move very well. Furthermore, the estimated position of the robot jumps around the real pose of the robot, and that makes even worse the localization. I've filtered the output of the EKF and this problem persists.

And sometimes, while I'm running the move_base with the AMCL I get this warning:

[ WARN] [1352195952.098814110, 3250.301000000]: The origin for the sensor at (182092.21, -2410790.70) is out of map bounds. So, the costmap cannot raytrace for it.

I only get this warning sometimes, don't know the reason. After this warning the robot tries again to move, so I think that the AMCL has made a jump outside the map but in the next iteration it has corrected itself. But it just a supposition, I don't know exactly why I get this warning or if that's the reason why I cant make work the EKF with the AMCL.

I've tested to use the encoder based odometry only and all this problems disappear, I have this problems only when I run the EKF and the AMCL simultaneously.

Does anyone have the same problem?

Here you have the parameters of the AMCL:

<launch>
<node pkg="amcl" type="amcl" name="amcl" output="screen">

  <remap from="scan" to="tof_camera/fake_laser" />
  <remap from="cmd_vel" to="summit_xl_ctrl/command" />

  <!-- Publish scans from best pose at a max of 10 Hz -->
  <param name="odom_model_type" value="diff"/>
  <param name="odom_alpha5" value="0.1"/>
  <param name="transform_tolerance" value="0.2" />
  <param name="gui_publish_rate" value="10.0"/>
  <param name="laser_max_beams" value="30"/>
  <param name="min_particles" value="500"/>
  <param name="max_particles" value="5000"/>
  <param name="kld_err" value="0.5"/>
  <param name="kld_z" value="0.999"/> 
  <param name="odom_alpha1" value="0.2"/>
  <param name="odom_alpha2" value="0.2"/>
  <!-- translation std dev, m -->
  <param name="odom_alpha3" value="0.8"/>
  <param name="odom_alpha4" value="0.2"/>
  <param name="laser_z_hit" value="0.5"/>
  <param name="laser_z_short" value="0.05"/>
  <param name="laser_z_max" value="0.05"/>
  <param name="laser_z_rand" value="0.5"/>
  <param name="laser_sigma_hit" value="1.0"/>
  <param name="laser_lambda_short" value="0.1"/>
  <param name="laser_model_type" value="likelihood_field"/>
  <!-- <param name="laser_model_type" value="beam"/> -->
  <param name="laser_likelihood_max_dist" value="2.0"/>
  <param name="update_min_d" value="0.6"/>
  <param name="update_min_a" value="0.5"/>
  <param name="odom_frame_id" value="odom_combined"/>
  <param name="base_frame_id" value="base_footprint"/>
  <param name="global_frame_id" value="map"/>
  <param name="resample_interval" value="1"/>
  <param name="transform_tolerance" value="0.6"/>
  <param name="recovery_alpha_slow" value="0.0"/> 
  <param name="recovery_alpha_fast" value="0.0"/>  

  <param ...