finally working pd-controller in vicon, hooray