Map update based on occupancy probability grid

[matteodv99tn]

The algorithm that I thought is greedy and could lead to high computational times, maybe other solutions can be found.

Inputs

It's assumed that we have a buffer of $n$ previous robot position and related observation vector: this allows us to store a buffer of all prior landmark observations.

Remark: such landmarks can also be not present in the map.

Each observation lead to a landmark characterized by a state estimate and the covariance matrix of such estimate. The idea is so to add a landmark to the map only if it has been seen in all $n$ previous scans within a certain probability. Already mapped landmarks of course must be discarted.

The algorithm

1. Initialize a cartesian grid representing the map (in global reference frame);
2. For each laserscan, assign an occupation probability based on the performed observation (using the uncertainty ellipsoid of the landmark);
3. Once the grid is computed, determine the spots for which there was 100% probability of having a features for all laserscans and perform a Bayes fusion (weighted least square) to initialize the corresponding landmark to the map.

More to follows...

[matteodv99tn]

Feel free to share your thoughts here, this was just a basic idea

[matteodv99tn]

At the current state all it misses is the deployment of the new features in the map This integration is important also w.r.t the EKF

[matteodv99tn]

Basic algorithm merged into main with pull request #14