Implement low-variance sampling algorithm

[nahueespinosa]

Description

From Probabilistic Robotics, Chapter 4.2.4:

[...] the resampling process induces a loss of diversity in the particle population, which in fact manifests itself as approximation error. Such error is called variance of the estimator: Even though the variance of the particle set itself decreases, the variance of the particle set as an estimator of the true belief increases.

The first proposed strategy for variance reduction is already partially implemented in beluga_amcl by not updating the filter in case the robot is not moving (note: there is still work to do if we want to reduce the resampling frequency by measuring the variance of the importance weights but that's for another ticket).

The second strategy consists in using a low-variance sampling algorithm for the resampling step. We should probably investigate if it's worth implementing in our first iteration of the library.

We also need to consider how this algorithm interacts with KLD resampling, since it seems to require the number of particles to be determined from the start.

Definition of Done

• A new range adaptor called beluga::views::low_variance_sample.

[ivanpauno]

We also need to consider how this algorithm interacts with KLD resampling, since it seems to require the number of particles to be determined from the start.

I'm almost sure it doesn't require that, you only require the previous number of particles.
I think that the algorithm described in Probabilistic Robotics uses a fixed number of particles, but that's not needed.
The number of samples taken may be more or less than the previous particles.

Another advantage of the method is that it only requires generating one random number, which may improve performance.

[nahueespinosa]

@ivanpauno When doing the following computation on each iteration:

$$ u = r + (m − 1) · M ^ {−1} $$

It seems to me that $M$ must be equal to the number of new particles you want to sample, not just the number of previous particles.
If you have a bunch of $M$ particles and compute the random number as:

$$ r = rand(0, M ^ {−1}) $$

Taking less than $M$ particles (imagine 50%) doesn't look like it will represent the distribution correctly, since the particles with the highest weight could be ordered at the end of the range.

[glpuga]

Indeed, LVS requires M to be known in advance for its qualities to be true in the form shown in Chapter 4, and M is the number of particles you want after resampling. As it is it's not fit for drop in replacement in line 8 of the implementation of KLD proposed in Table 8.4 .

At it heart, LVS make sure the first random number it generates offsets a "sampler" that moves from start to end of the weighted sequence of particles, so that:

1. The probability of any given particle to be sampled is proportional to its weight.
2. All samples are systematically considered by the sampler, to minimize the chances that individuals can be dropped because of sheer bad luck during a sampling iteration. They can still be dropped if the sampler "jumps over" them, but a sample will have guaranteed survival if their weight is >= $1/M$.

Now, say we replace M by $M_min$ and proceed using LVS in line 8 of Table 8.4. We continue like this until $m$ reaches $M_{min}$; when it does, if KLD may still need to continue producing new samples, but LVS has ran out of tape, so we just reset the LVS algorithm by setting a new random offset $r$ and starting using $\mbox{mod}(m, M_{min})$.

The main drawback I can think of this is that if $M_min$ is large compared to $M_\chi$, the sampler will be slightly biased towards sampling particles earlier in the list a bit more that the ones at the end. This can probably be addressed by changing the range of the random offset $r$ to be $M_{min}$ instead of $1/M_{min}$, and sampling $u = \mbox{mod}(r + (m-1) * M_{min}^{-1}), M_{min})$.

That would probably keep the good qualities of LVS while making it fit for use with KLD at the expense of a bit more complexity in the sample selection algorithm, but a more detailed analysis should be done to ensure that's the case.

[glpuga]

As a follow up and in order for me not to forget it, a more elegant solution to the bias-to-sample-the-first-ones problem of the LVS-for-KLD proposal above is to randomize the sampling order with a full cycle pseudorandom number generator.

A FC PRNG is useful for many things, but one way to think about it is like a counter that is guaranteed to go through all the numbers in its range (for LSFR, all but 0), but doing so in a pseudorandom order.

LSFRs PRNGs are trivial to implement (literally xor operation and shift), and full cycle counters are tabulated for different cycle lenghts.

The main changes would be, the LVS value for M should be the largest cycle C that's still smaller than $M_{min}$. Instead of starting with $m = 0$ you reset the LSFR at the start, and move it forward for each new new sample you extract. Once you extracted C samples, you get a new offset $b$ and reset the LSFR (or not, actaully LSFRs are periodic so...). This way, instead of sampling always "from left to right" along the particles container, you systematically sample them but in a pseudorandom order.

[nahueespinosa]

It's worth noting that we don't need to solve the LVS-KLD compatibility problem. From an implementation standpoint, we can simply use low-variance sampling with the existing fixed resampling policy.

Regarding the proposals, I think we could devise an experiment to test how population diversity evolves over time with different algorithms and distributions. And there are probably specific techniques and metrics that statisticians use to verify that their algorithms are behaving as expected that we could definitely delve into if necessary. Not saying that we should though.

[hidmic]

This way, instead of sampling always "from left to right" along the particles container, you systematically sample them but in a pseudorandom order.

Isn't it the same 🤔? Order is arbitrary.

Since KLD resampling works with a lower bound on particle count, we could also sample in batches i.e. apply low variance sampling N times to the same M particle approximation of the proposal distribution until the KLD condition is satisfied.

[nahueespinosa]

As of today, this is a low-hanging fruit for anyone that wants to contribute by implementing a new range adaptor called beluga::views::low_variance_sample.