Added matrix class to help with calculations used in the kalman filter