Changement de repere et quaternions

Bonjour,
J'essaie désespérément d'effectuer un changement de repère entre deux capteurs. Chacun a 6 coordonnées absolues (x,y,z, Azimuth(rotation sur z), Elevation (rotation sur y), Roll (rotation sur x)). Mon objectif est d'exprimer les 6 coordonnées d'un capteur en fonction du 2e. Voici comment je procède:

On a:
Capteur 1 : x1,y1,z1,A1,E1,R1
Capteur 2 : x2, ..., R2

Mon programme: (C++)

P = (x2,y2,z2)
Pref =(x1,y1,z1)

O = quaternion obtenu à partir des angles d'Euler du capteur 2
Oref = idem pour le capteur 1

Où, pour des angles (A,E,R) le quaternion d'orientation O est ainsi obtenu (formule directement pompée d'internet)
O (a,b,c,d)
où
a = (cos(R/2) * cos(E/2) * cos(A/2) + sin(R/2) * sin(E/2) * sin(A/2)),
b = (sin(R/2) * cos(E/2) * cos(A/2) - cos(R/2) * sin(E/2) * sin(A/2)),
c = (cos(R/2) * sin(E/2) * cos(A/2) + sin(R/2) * cos(E/2) * sin(A/2)),
d = (cos(R/2) * cos(E/2) * sin(A/2) - sin(R/2) * sin(E/2) * cos(A/2))

Oref_inverse = inverse(Oref);
trans = P-Pref
Qtrans = (0,trans.x, trans.y, trans.z) //quaternion obtenu à partir du veteur trans.
Qtrans = Oref_inverse * Qtrans * Oref

//OBTENTION DE LA TRANSLATION
trans = (Qtrans.i, Qtrans.j, Qtrans.k) //vecteur des 3 composantes complexes de Qtrans

//OBTENTION DES ANGLES
O = Oref_inverse*O;
et on extrait les trois angles d'Euler à partir de O

Le pb est que les résultats sont faux: si je fixe les 2 capteurs sur un solide et que je bouge le solide, les coordonnées relatives devraient être fixes (les 2 cpateurs étant fixes l'un par rapport à l'autre), or elles ne le sont pas... ='(

SVP, je suis en train de péter les plombs, dites-moi ce qui ne va pas!!!

Merci bcp d'avance !