Feature/kalman

[szeyoong-low]

Noise filtering and imputing of vanished frames using Kalman filters

• Implemented multivariate Kalman filters for robots and ball, integrated into the PositionRefiner.

The methodology is described in detail in the documentation. When running in rSim, the optimal parameters are the arguments passed to the rSim noise generator. However, this should be calibrated based on real-world conditions when live robots are used. The operation of the filters is toggled by a flag to the StrategyRunner.

• A helpful side effect is that the filters can predict the positions and orientations of robots with missing position data ("vanishing").
• Modified the StrategyRunner to pass the last GameFrame into the PositionRefiner, so that it can be used by the filters.
• Wrote an example strategy, PointCycleStrategy to be used as a "gold standard test". It makes a single robot move in a cycle amongst 10 positions on the field.
• Wrote Python scripts to run analytics on the filters. These have been moved to the testing/data_processing branch.
• Created some helpful math utilities to support the filter's implementation.
• Fixed unit tests that broke due to the additional arguments required by the PositionRefiner.

Results achieved

Robot positioning accuracy improved by over 35%, in terms of the reduction in mean squared errors in x coordinates, y coordinates, and the norm of the residual vectors. The following results were obtained when running the PointCycleStrategy in rSim with the DWA motion controller under Gaussian noise of 1cm standard deviation (near the system's threshold) for over 2.5 minutes.

Here's a subset of the simulator data, showing the reduction of errors for individual frames.

The filters work great for orientation data too, achieving a 54% improvement in accuracy. The following polar plot shows simulator data captured when running the PointCycleStrategy in rSim with the DWA motion controller under Gaussian noise of 5 degrees standard deviation for over 2.5 minutes. Notice how the fluctuations are reduced.

Similar results were obtained with the ball.

Completed by @szeyoong-low and @AndrewAha