added D term for drift, changed D term for height, make sure to use test latest code so plug and go
Merge branch 'master' of github.com:jhauswald/koala