Removed vector and fixed size array operations from Matrix. Changed ordering of definitions in kalman.h
Moved L3G4200D from globals.h to devices.h
Performance updates and remove weird self-assignment