Całkowanie kwaternionuwektorem prędkości kątowej z gyro

[smarq8]

Witam.
Ostatnio zacząłem się bawić żyroskopem (MPU6050) i regulatorem PID. Kąt żyroskopu w przypadku obrotu wokół jednej osi jest banalnie prosty (AngleX += GyroX; // pomijam tutaj kwestię dryfu i jednostek), schody zaczęły się kiedy chcę już policzyć kąty dla 3 osi jednocześnie - chyba nie muszę tłumaczyć dlaczego wcześniejszy wzór nie będzie działać. Poczytałem trochę w sieci i zapoznałem się mniejwiecej z tworem jakim są kwaterniony i ich algebrą. Na większości stron podają wzór jak aktualizować kwaternion za pomocą wektora prędkości kątowych.

Kod: Zaznacz cały

dq(t) /dt = 1/2 * W(t) q(t)

Jednak z moich symulacji wynika, że dane wynikowe nie pokrywają się z przewidywanymi np zakładając, że W=[1,0,0], q=[1,0,0,0] to wynikiem jest 0,927295218 radiana w osi X zamiast 1.
Tutaj podaję przykład mojej implementacji ww wzoru.

Kod: Zaznacz cały

Q_ApplyVelo4(q,x,y,z){
	qg := [0,x,y,z]; [0,1,0,0]
	q0 := [1,0,0,0]; [1,0,0,0]
	
	q2 := Q_MulR(qg,0.5); [0,0.5,0,0]
	q3 := Q_MulQ(q2,q0); [0,0.5,0,0]
	q4 := Q_AddQ(q3,q0); [1,0.5,0,0] ??????????
	q5 := Q_Normalised(q4); [0.8944271910,0.4472135955,0,0]
	return q5
}

Ponad tydzień główkowałem gdzie popełniam błąd i próbowałem najróżniejszych wzorów (w wersji z macierzą itp) aż doszedłem do takiego:

Kod: Zaznacz cały

q = cos(a/2) + i ( x * sin(a/2)) + j (y * sin(a/2)) + k ( z * sin(a/2))

oraz moja implementacja

Kod: Zaznacz cały

Q_ApplyVelo(q,x,y,z){
	r := angular_rate_to_quaternion_rotation([x,y,z],1)
	q := Q_MulQ(r,q)
	q := Q_Normalised(q)
	return q
}
angular_rate_to_quaternion_rotation(w, dt)
{
    wx := w.1
    wy := w.2
    wz := w.3
    l := sqrt(wx*wx + wy*wy + wz*wz)
    dtlo2 := dt * l / 2
    cl2l := cos(dtlo2)
    sl2l := sin(dtlo2)/l
    q0 := cl2l
    q1 := sl2l * wx
    q2 := sl2l * wy
    q3 := sl2l * wz
	
	return [q0,q1,q2,q3]
}

I tylko w tym przypadku obliczenia w pełni pokrywają z oczekiwaniami.

I tu pojawia się moje pytanie, czy ja w pierwszej metodzie popełniam jakiś błąd? Czy może przed normalizacją nie należy dodawać kwaternionów do siebie tylko coś innego?
... rvelocity/
w dziale "Representing angular velocity using quaternions" także opisują ten wzór lecz w przykładowej ramce na niebiesko poniżej ni z tego ni z owego używają drugiego wzoru

PS. jestem także ciekaw czy da się jakoś całkować dane z żyroskopu w 3 osiach nie używając kwaternionów lub DCM? Coś na kształt (AngleX += GyroX;).