Added more matrix methods. Implemented filtered gyro readout.