Cleanup, removed obsolete sources
Added simple PID-Controller for Position
Added squareRoot
Adjust integration part of PID to pos-error
Switch x/y of Target
Improvements to PID, resetting setpoints complet and stop integration
while moving setpoints
fix sign
Rotate rarget vector only over yaw