Algorytm obliczeniowy - Platforma Stewarta 6 DoF

[Pasjonat]

Witajcie,
mam następujący problem, mianowicie zbudowałem mechanicznie platformę Stewarta 6 DoF, wyznaczyłem macierze transformacji pomiędzy i-tymi układami bazy a układem centralnym bazy oraz analogicznie dla platformy. Na tej podstawie wyznaczyłem macierze transformacji pomiędzy i-tym układem platformy i i-tym układem bazy jednak mam problem, gdyż mój algorytm nie działa poprawnie, to znaczy, iż układ nie reaguje poprawnie (reakcja jest, "prawie" sensowna, lecz na przykład przy podaniu kąta obrotu układ nie obraca się tylko wokół tej zadanej osi) na zadawanie kątów obrotu układu platformy względem układu centralnego bazy bądź to samo dla wektora przesunięć [x,y,z]. Próbowałem wyznaczyć przestrzennie zależność kąta obrotu członu aktywnego (mocowanego na wale napędu, w moim przypadku na wale serwomechanizmu) od innych wielkości, taka jak wspomniana długość pomiędzy i-tym układem bazy a i-tym układem platformy etc. Poszukuję porządnie opisanych algorytmów obliczeniowych, ale nie przesadnie zagłębionych w matematyce, gdyż chcę zrobić prostą implementację na 32-bitowy mikrokontroler, w tym przypadku z rdzeniem Cortex-M4. Będę wdzięczny za wszelkie sensowne porady. Dodam tylko, iż kinematykę platformy symuluję na podstawie modelu CAD. Konstrukcję platformy przedstawiam w załączonym zdjęciu.

Pozdrawiam.