Это, если я не ошибаюсь, уже PIFF регулятор, где дифференциальная составляющая заменена уставкой с определенным весом
Примерно так, только у ПИД изменение дифференциального коэффициента будет инертным. А если сделать наоборот, линейный регулятор по угловой скорости и к нему добавить ПИ по азимуту, кмк будет более точное движение