Cleaned up matrix functions