Да простых путей мы не ищем.
Я вод думаю с чего начать.
Ну ладно предположим, что твоя ракета не качается вокруг своей оси, и тебе не нужно вращать трехмерные матрицы. Очень, очень предположим.
Начнем с того что пропорциональное управление не годится от слова совсем.
Во первых при увеличении скорости в два раза управляющий момент при одном и том же отклонении рулей будет расти в квадрате. То есть угол отклонения должен меняться от скорости.
Во вторых функция обратной связи должна быть похожей на ПИД регулятор.
Я бы сказал адаптивный алгоритм похожий на ПИД.
Из простых советов – ракета должна быть длинной и жесткой.
Вопрос. Алгоритм ты брал готовый от квадрика или сам писал?