# Problems with ekf + amcl: particles cloud diverges

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 ...
edit retag close merge delete

Sort by » oldest newest most voted

I guess the output of robot_pose_ekf is inconsistent or you have a problem with your laser sensor. A few things I would try:

• Try use AMCL together with unfiltered odometry. If it is working, the problem lies in your EKF implementation. Otherwise it is probably caused by your laser sensor.

• Make sure your units are correct, i.e. the output of odometry (and of the filtered odometry coming from the EKF) must be in meters, velocities must be in m/s. Laser range data must also be in meters.

• Make sure that your laser sensor is configured properly. Visualize it using rviz and check if obstacles appear at correct positions.

• If the problem lies in EKF, consider disabling GPS and only use IMU and raw odometry. Consider trying robot_pose_ekf just to verify that the problem is not caused by the integration of GPS.

more

The problem must lie in the EKF, because with unfiltered odometry it works fine. The laser is not the problem, because when I visualize it with rviz all seems to be ok. When I use the EKF without the GPS the problem disappears, but then I loose the absolute reference for localization

( 2012-11-06 21:43:05 -0500 )edit

When adding GPS does maybe the behavior of your covariance matrix change? I'm not sure if AMCL actually uses it, it's just a guess. When using GPS, are your coordinates maybe really big? Can you maybe map them to some artificial origin close to your location to avoid possible numerical problems?

( 2012-11-06 21:49:16 -0500 )edit

Well, I used a simplified version of the EKF, and the problem was solved. So as you said, the problem lies in the EKF, but don't know why. Using the big UTMs coordinates was not the problem, with this new version I use the same coordinates and all works fine. Thanks anyway for the hints ;)

( 2012-11-19 22:32:46 -0500 )edit

use AMCL's filter and do the sensor fusion by raw data. it doesn't really make sense to use an already filtered pose as input.If you already have a map and would like to localize your robot in it, you should use amcl_pose. As for the /odom topic, it is subscribed by move_base not for the pose information but for the velocity

more

1

AMCL and the EKF are definitely not doing the same thing. It is really common to use them together, e.g. on the turtlebot or the PR2 (without GPS though). AMCL does particle-filter based localization using laser and odometry, i.e. it uses features of the environment for global localization.

( 2012-11-06 00:08:58 -0500 )edit

Sorry, I mean that they both filter the odometry data

( 2012-11-06 00:12:28 -0500 )edit