Just improved the PID contoller part:
now including: setpoint weighting, derivative filtering and anti-windup --> improved stability for setpoint change (robot speed)
PID derivative term now ready to be used for optimization (currently not used) ... will try to come up with a working parameter set soon ;)