corrected small bug in the size of the final Jacobian matrix.
By default Eigen matrices are initialize to Zero (main reason is the Jacobian)