executes but still Jacobian problems and need to tune gains