Merge pull request #45 from mlosch/master

Extend goto controller to be able to use path following.
Merge pull request #48 from mlosch/master

Hard reset controllers when imu sends signal
some bugfixes