function y=pqrQ_V_P(u); global vitesse_rotation % Fonction qui extrait : % les dérivées des angles de Cardan % les angles de Cardan du repère satellite par rapport au repère orbital % du vecteur d'entrée u à 7 composantes % de 1 à 3 p q r composantes absolues du satellite exprimées en repère orbital XYZ % de 4 à 7 les 4 composantes du quaternion d'attitude du satellite xyz par rapport % au repère orbital XYZ % ------------------------------ But ---------------------------- % Eviter les problemes de dérivées avec sauts dans les PD ou PID % ---------------------------------------------------------------- % ********************* DEBUT PROGRAMME ************************ omega_abs=[u(1);u(2);u(3)]; Q=u(4:7); q0=Q(1);q1=Q(2);q2=Q(3);q3=Q(4); % Extraction des angles en radian phi=atan2(2*(q0*q1+q2*q3),q0^2-q1^2-q2^2+q3^2); teta=asin(2*(q0*q2-q1*q3)); psi=atan2(2*(q0*q3+q2*q1),q0^2+q1^2-q2^2-q3^2); angles_colonne=[phi teta psi]'; % Sortie vecteur colonne roulis tangage lacet % Calcul de la matrice de passage de XYZ à xyz Pmat=passage(Q); % Calcul des composantes de Y sur les axes satellite xyz, ceci pour transformer % la rotation orbitale woY dans xyz rotation_orbitale=Pmat*[0 vitesse_rotation 0]'; % Calcul de la matrice de changement d'axes de mesure des angles de Cardan vers xyz ( elle % n'est unitaire et présente une difficulté pour tangage = + ou - 90° % D'où la précaution suivante qui ne faussera les calculs que sur un court instant % lorsque cos(tangage) approche 0 on donne des valeurs plausibles mais inefficaces % pratiquement nulles if abs(cos(teta))>0.001 Pcardan=mat_card(angles_colonne); else % Pcardan=eye(3)*1000; Pcardan=[1 0 -sin(teta); 0 cos(phi) 0.001*sin(phi); 0 -sin(phi) 0.0001*cos(phi)]; end % Calcul des vitesses phi' teta' psi' en colonne vitesses_colonne=inv(Pcardan)*(omega_abs-rotation_orbitale); % Sortie d'un vecteur colonne à 6 composantes: % De 1 à3 les angles de Cardan en rd % De 4 à 6 les vitesses angulaires dérivées des angles de Cardan y(1:3)=angles_colonne; y(4:6)=vitesses_colonne; y=y'; % Sortie colonne end % -------------------------------------- Fin programme -----------------------------