[✨FEAT✨]: Cone Mapping Performance Increase

[Zane-Larking]

Is there an existing enhancement request for this?

• [X] I have searched the existing feature requests

Description

Currently, within the 'sort_and_add_cones' function, the complexity of comparing one cone perception data point is O(n). Comparing all cones to all measured data point is O(n²). This has clear areas of improvement. Please implement a means to decrease the search space of the cones that a data point needs to be compared against.

Additional Information

Please consider:

• Utilizing pre-existing information we are not exploiting (color, etc...)
• Implementing an easier to search data structure.
  • Space partitioning algorithms.
  • Clustering algorithms.
  • Other search space reduction algorithms.

We are expecting to utilize at minimum 60 total cones for a physical track, and otherwise can't afford to field more than 100. An algorithm should time efficient over this range of dataset sizes.

[Zane-Larking]

There may be a bit of interplay between this Issue and #85.