Implemented matrix library. Finalized Kalman filter tuning. Fixed Watchdog bug. Added line number to CatchPrint.