Switched to Eigen for matrix math