Integrate sensor probabilities across resampling iterations

[glpuga]

Related to #57.

Summary

• 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.

Checklist

• Read the contributing guidelines.
• Configured pre-commit and ran colcon test locally.
• Signed all commits for DCO.
• Added tests (regression tests for bugs, coverage of new code for features).
• Updated documentation (as needed).
• Checked that CI is passing.

[nahueespinosa]

@glpuga This is great! Thanks for the documentation fixes and minor refactors.

[glpuga]

Ready for another view. I did not update the tolerances since I could not get the system_test to run. I opened a ticket about that, since the error seems to be in main.

[nahueespinosa]

This is not exactly what AMCL does. I think you need to cube the result of:

index < likelihood_field_.size() ? likelihood_field_[index] : unknown_space_occupancy_prob

And then accumulate the results by adding them, not multiplying them.

[glpuga]

yes, you're right. My mistake. I'll fix it.

[glpuga]

The longer you look at it, the weirder it becomes. The accumulation starts from 1.0, not from 0.0.

// Compute the sample weights
for (j = 0; j < set->sample_count; j++) {
    // ...
    p = 1.0;
    // ...
    for (i = 0; i < data->range_count; i += step) {
        // ...
        pz = 0.0;
        // ...
        pz += ...;
        pz += ...;

        assert(pz <= 1.0);
        assert(pz >= 0.0);

        p += pz * pz * pz;
    }

    sample->weight *= p;
    total_weight += sample->weight;
}

Also, the Beam Model does the same. #99

[ivanpauno]

The accumulation starts from 1.0, not from 0.0.

That looks like they started by multiplying and then and some point switched and forgot to change the starting value...

But starting with a weight of 1 also makes sure all particles are somehow weighted, so it's a big difference to have that in the addition or not (my assumption is that all other terms of the sum are less than 1., that's why I consider starting with 1 a big difference).

[nahueespinosa]

LGTM!

[glpuga]

@nahueespinosa I have not yet run system_test and checked if the tolerances need ajustments. Should I

1. merge this and check/fix the tolerances in a follow up PR.
2. wait until we define the lfs ticket and check/fix as part of this MR?
3. not wait, sort the configuration issue locally and check/fix as part of this MR?

[nahueespinosa]

@ivanpauno Could you run a benchmark on this branch? I have to rebuild the dockers to do it myself.

@glpuga I have run the system test locally and it consistently passes. So merging this as is is also a good option in my opinion.

[glpuga]

Ok! I'll wait for @ivanpauno JIC.

[ivanpauno]

@ivanpauno Could you run a benchmark on this branch? I have to rebuild the dockers to do it myself.

Sure, I think I can do it for tomorrow.

[ivanpauno]

LGTM!

[nahueespinosa]

Benchmark results, red is before this patch and blue is after this patch. The error is a bit higher, but that's fine by me.