Revisit recovery algorithm implementation

[hidmic]

Feature description

#279 uncovered a few discrepancies between our implementation of the particle degeneracy recovery algorithm and that of Nav2 AMCL. We should revisit both control flow and normalization factors in our implementation.

Implementation considerations

Some notes and hints that may be relevant:

• Make sure to update the probability estimator every time we propagate
• Compare normalization using distribution mass, mean, and max.
• Consider grabbing weights before multiplicative integration.