Adding integrators to position controller

[mzahana]

@Jaeyoung-Lim

I was thinking if it makes sense to add integrators to posController. This allows to get rid of steady state errors (e.g. caused by disturbance).

You can check Eq. 7 in this paper

[Jaeyoung-Lim]

@mzahana Would be interesting to see how the performance is affected. I did not add a integrator since I was aiming for more dynamic maneuvers in this implementation, but if you can make it work reliably we can make it optional.