Refactored matrix calculations