switched to Eigen matrices, compiles and appears to run correctly