[Autonomy] Develop EKF Localization Algorithm

[SwapnilPande]

Which section of robot code is this for?
SLAM

Description of feature

• Build an extended Kalman filter algorithm to determine the position of the robot based on the data input from the AruCo ROS node.
• Build ROS node to collect data and publish estimated robot location

Additional Notes
This is the first step required for localization. Before attempting to build a full SLAM algorithm including sensor fusion and localization relative to obstacles on the field, we want a simple localization algorithm based only on the AruCo marker input. After this is completed, we can continue to augment this system by adding more sensors and building out a full EKF SLAM algorithm.

[alexbarnett12]

We have built the ROS node with the EKF. We need to run unit tests. @partlygloudy we'll need your help determining the best way to run unit tests for the ROS node.

[partlygloudy]

Awesome, we've got a working aruco localization ROS node as well so we can definitely do some testing early this week