Fixed multiplication error
Added Eigen library
Invert jacobian function completed