Tolerance relaxation

[alphaville]

In the penalty method we currently use the same tolerance for all instances of the inner solver. We can start with a higher tolerance, epsilon_0, and decrease it by multiplying by a 0<beta<1 (e.g., 0.1) until it reaches the target tolerance. Overall, this might save iterations in some cases.

This should be optional (for now) by setting the default initial tolerance equal to the target tolerance and allow the user to override this. We can later see whether it improves things and determine a meaningful initial tolerance and update parameter.

[korken89]

Sounds good, I was playing with this and saw reduction in iterations for MAVs when playing manually with this.

[alphaville]

@korken89 I noticed the same - and you can start with a very high tolerance (e.g., 0.1 or 1.0) and an update rate of 0.1.