The mapping itself was seprated out into a calls because it was reused
for the experimental nudging implementation.
The position resets which were handled correctly before now
change the wrong setpoints and I adjusted.
The nudging has to be before any filtering, then these member setpoints
which are essentially copies are not needed anymore.
The velocity setpoint of the position controller
does a jump when unlocking position with a non-zero position error.
This is solved by using the velocity setpoint feedback to smoothly
take over.
Matthias Grob