Modified pd_controller to work with ekf function.  Still requires a call to getpose() after every rotation.