Sensor hil

I have sensor level hardware in the loop working on this branch. This required a performance overhaul of the extended kalman filter (currently auto-generated matlab c code). I ended up writing a C++ matrix/ vector wrapper around the arm accelerated CMSIS libraries. It also required a rework of the fixed wing control code. I made the control blocks more OO and added various utilities for dealing with UOrb in an OO manner.