Vector and 3x3 Matrix math
Refactored PID Controller
Unit tests for Math library
Prototype Kalman filter for roll and pitch estimation