Added velocities. Also removed using namespace Eigen because it conflicted with other code. Had to commit out dampedLeastSquares code to get things to compile.