Математическая модель - редкостный быдлокод. Оперировать с векторами и матрицами нужно в терминах векторов и матриц, а не разваливая все на отдельные компоненты.
Чтобы понимать то, что вы делаете, вам придется вспомнить две дисциплины - дифференциальную геометрию и линейную алгебру - то есть то, что составляет курс "Теоретическая механика".
Первоначально, вам следует вышвырнуть все эти красивые абстракции и просто разобраться с механикой, которая у вас есть и с тем, на что вы можете влиять.
Двигатели постоянного тока, говорите? А какие датчики на них есть? датчики тока? Энкодеры на валу?
В зависимости от этого, вы можете подумать над тем, как вы можете управлять этими двигателями. Можете ли вы, например, задавать и контролировать момент на каждой оси? И так далее.
Знаниями нужно пользоваться, а не стремиться все упростить и скрыть от себя за абстракциями.
