ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

AMCL Particle Weights Are Always Equal - Why?

asked 2017-11-10 09:31:32 -0500

ElizabethA gravatar image

updated 2017-11-10 10:48:15 -0500

I'm driving the Turtlebot around the turtlebot_world Gazebo environment. I'm printing out the AMCL particle filter poses and the weights assigned to the poses, and when I drive around for a while (as long as 5 minutes), the weights assigned to the poses in the particle filter are all set to the same value (particle weight = 0.00196, for 500 particles, so they sum to 1, of course).

I expected there to be some variety in the particles' weights - some higher, some lower. My theories as to why they're all equal:
1. I could be printing the weights at the wrong point of the AMCL code
2. It's a demo Gazebo world and a demo AMCL map - is the set-up too "perfect" for different hypotheses to result in different weights or something?



Here's a code snippet:
File: AMCL/src/amcl_node.cpp

Function: AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)

~Line 1245, Added the following:

for(int i=0;i<set->sample_count;i++) {

   ROS_DEBUG("Weight: %f",set->samples[i].weight);
}

edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
1

answered 2017-11-10 22:45:32 -0500

updated 2017-11-10 22:47:02 -0500

Particle weights are supposed to be different, but AMCL resamples every resample_interval_ which could be every step. Regardless, you are indeed getting your data after the measurement update step but also after resampling. After resampling every particle weight is the same. Try looking at the particles after sensor data is integrated. line 1235 before the resampling in the next line

edit flag offensive delete link more

Comments

Great catch! I moved the print statement to line 1235 before the resampling, and now I'm seeing variety in the particle weights. Woohoo! Thank you.

My resample_interval_ parameter = 1, so it's resampling at every step.

ElizabethA gravatar image ElizabethA  ( 2017-11-11 08:34:21 -0500 )edit

great! please mark the answer as correct!

PeterMilani gravatar image PeterMilani  ( 2017-11-15 23:05:52 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2017-11-10 09:31:32 -0500

Seen: 843 times

Last updated: Nov 10 '17