Integrate GPS into Navigation module

[Iyury1]

Summary

The Navigation module currently reads from the acceleration from the BNO055 and uses that information along with an initial starting point to predict the motion of the pod using kinematic equations. The accuracy of motion prediction can be vastly improved by combining a position measurement from the GPS module. To do so, we will implement a sensor fusion technique to combine the GPS measurement with kinematic equation predictions. As a first step, we can use the Linear Kalman Filter algorithm to achieve sensor fusion.

Acceptance Criteria

• [x] Write geodesic coordinate transformation in CPP for Teensy Nav module
• [x] Write linear kalman filter equations in CPP for Teensy Nav module
• [x] Write tests for coordinate transformation and kalman equations
• [ ] Complete test plan and documentation

[Iyury1]

Estimator class is completed: https://github.com/albertaloop/T_SWE_2019_2020/tree/Ian-Navigation/Firmware/Navigation_Module_Teensy3.6

Some tests for estimator class: https://github.com/albertaloop/T_SWE_2019_2020/tree/Ian-Navigation/Firmware/Navigation_Module_Teensy3.6/navmod_tests

While testing GPS integration, unable to get a proper readout from the GPS sensor while running a timer interrupts for estimator. I think the issue is that the Serial class empties it's output buffer whenever an interrupt occurs. We should consider looking for a different sensor that doesn't rely on Serial. I will add some test results showing this problem soon.