Filter construction - deconstruction issues

[ulassbin]

Currently the algorithm is facing several issues with filter handling. Those are:

1. At the start 10 filters are started The image below shows that, in underscore bag. 9 other fields are empty, thus we are starting 9 filters for no reason.
2. No additional filters are generated on the runtime. It is known that there might be more than 10 objects at the time, however sensor provides 10 matches at each time. Given I track N filters, I can compare the new 10 readings to those N filters. For the distinct ones, I can restart a filter by pushing it to the list and directly assigning the values.
3. To handle the filters from scaling indefinitely, I can compare each filter with each other, if their values are close enough I can erase the duplicates.(This might not be the best way to compare in terms of efficiency, this can scale quite fast.)

[ulassbin]

It is clearly visible from image above, only the 0'th element started from a position different than 0. This means initial message from the sensor hold only 1 object, the other ones were empty but I started 9 of them in 0,0.

[ulassbin]

Randomly initializing 10 filters issue is handled here: 7f5a8c0441cae87e128fcf899f1de34d3e5f0468

Also, some issues related with gating are fixed as well.

There still seems to be an issue about covariance calculations, covariance value at one direction does not grow as quick as the other one. Need to check matrix dimensions. Sometimes, tracks do crazy stuff, like moving towards a random target. Visualizing connections can be helpful to track that issue.

[ulassbin]

Crazy stuff like random moving target is caused by gating several obstacles far away. Now the node visualizes correspondences as wel, here is a screenshot of a very large correspondence acceptence. The lines show correspondence vectors.

Still covariance increases in prediction step seems messed up.