Changed the diagonal to be a VectorXd