3d Kalman filter progress + minor sch

-moving towards a matrix (CMSIS) based kalman implementation
-cosmetic schematic  changes