Adjust Setpoint

x [m]
y [m]
z [m]
yaw [deg]