Update QP controller

maybe controlUtil.h (camelCase?)