Changed PD control to not depend on old values.
Merge branch 'hydro-devel' of github.com:tonybaltovski/apm-ros into hydro-devel