Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Reduce the resampling frequency #57

Closed
4 of 5 tasks
nahueespinosa opened this issue Jan 10, 2023 · 2 comments
Closed
4 of 5 tasks

Reduce the resampling frequency #57

nahueespinosa opened this issue Jan 10, 2023 · 2 comments
Assignees
@glpuga
Labels
cpp Related to C++ code enhancement New feature or request ros Related to ROS

Comments

@nahueespinosa
Copy link
Member

nahueespinosa commented Jan 10, 2023 •
edited
Loading

Description

As mentioned in #48, to avoid loss of diversity in the particle population, it is recommended to reduce the resampling frequency. A standard approach to determining whether or not to resample is to measure the variance of the importance weights. AMCL has a parameter to set the number of filter updates required before resampling.

Several things need to change in our implementation in order to add this functionality.

Definition of Done

  • Compute the weighted estimate of the particles, not assuming all particles have the same weight (to publish the correct estimate from integrated measurements).
  • Integrate multiple measurements by multiplicatively updating the importance factor or weight.
  • Trigger the resampling step using a counter (see nav2_amcl/src/amcl_node.cpp#L174-L176).
  • Trigger the resampling step by computing the effective number of particles (*).
  • Trigger the resampling step by measuring the variance of the importance weights (**).

Additional considerations

(*) See #85 (comment).

(**) This is an extension mentioned in Probabilistic Robotics to what nav2_amcl does, so maybe we should consider adding this to a new localization node and keep the original settings in beluga_amcl.
@nahueespinosa nahueespinosa added cpp Related to C++ code ros Related to ROS labels Jan 10, 2023
@nahueespinosa nahueespinosa mentioned this issue Jan 10, 2023
Closed
@nahueespinosa nahueespinosa added the enhancement New feature or request label Jan 10, 2023
This was referenced Jan 25, 2023
Closed
Merged
nahueespinosa added a commit that referenced this issue Jan 27, 2023
@nahueespinosa
Refactor laser_callback method (#89)
6a7c166 
This is directly related to #57, but it should make #40 much easier to
implement. It is a small refactor of the `laser_callback` method in
`amcl_node.cpp`. The functional changes are:

- We now get different errors depending on which transform lookup
  failed.
- We update the sensor and motion models with every laser scan message,
  if the robot has not moved we only skip the resampling step.
- We publish a new estimate after every update of the filter.
- Removes `last_odom()` method from the differential drive model
  interface.

Signed-off-by: Nahuel Espinosa <[email protected]>
@glpuga
Copy link
Collaborator

glpuga commented Feb 9, 2023

Literature review: https://bisite.usal.es/archivos/resampling_methods_for_particle_filtering_classification_implementation_and_strategies.pdf

@glpuga glpuga mentioned this issue Feb 15, 2023
Merged
6 tasks
glpuga added a commit that referenced this issue Mar 6, 2023
@glpuga
Add new resampling policies (#119)
91dc418 
Related to #57 

- Refactors minimum motion check into a resampling policy
- Adds a policy that implements the "resampling_interval" parameter.
- Adds a policy that implements the "selective_resampling" parameters.

Signed-off-by: Gerardo Puga <[email protected]>
This was referenced Mar 26, 2023
Closed
Merged
glpuga added a commit that referenced this issue Apr 1, 2023
@glpuga
Integrate sensor probabilities across resampling iterations (#149)
5db2870 
Related to #57.

- Initializes weight to 1
- Incorporates previous weight to the new weight calculation during the
importance weight step.
- Corrects importance_weight formula
- Updates related tests
- Minor documentation and testing updates.

Signed-off-by: Gerardo Puga <[email protected]>
@glpuga glpuga mentioned this issue Apr 7, 2023
Merged
6 tasks
glpuga added a commit that referenced this issue Apr 15, 2023
@glpuga
Add WeightedStateEstimator2d mixin (#161)
137763d 
Related to #57 

- Refactor `estimate()` function to use weights instead of assuming
uniform weight for samples
- Adds WeightedStateEstimator
- Refactors SimpleStateEstimator to work as a subcase of estimation
where all weights are 1.0.
- Add testing for WeightedStateEstimator and also do a few minor
refactors to the SimpleStateEstimator test cases.
@nahueespinosa
Copy link
Member Author

I think we can consider this one closed by #119, #149 and #161.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees

@glpuga glpuga

Labels
cpp Related to C++ code enhancement New feature or request ros Related to ROS
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
2 participants
@glpuga @nahueespinosa