PSINS中欧拉角、方向余弦矩阵与姿态四元数的转换公式与代码

提示：代码只给出PSINS工具箱中的形式，由于本文所给公式与PSINS中使用的有差别，因此代码与公式不一定对应，但这仅仅是因为定义姿态角矩阵时，三个欧拉角的顺序有关，因此问题不大，读者自行判别，根据需要更改即可

欧拉角转四元数

公式

代码

function qnb = a2qua(att)
% Convert Euler angles to attitude quaternion.
%
% Prototype: qnb = a2qua(att)
% Input: att - att=[pitch; roll; yaw] in radians
% Output: qnb - attitude quaternion

   att2 = att/2;
    s = sin(att2); c = cos(att2);
    sp = s(1); sr = s(2); sy = s(3); 
    cp = c(1); cr = c(2); cy = c(3); 
    qnb = [ cp*cr*cy - sp*sr*sy;
            sp*cr*cy - cp*sr*sy;
            cp*sr*cy + sp*cr*sy;
            cp*cr*sy + sp*sr*cy ];

欧拉角转方向余弦矩阵

公式

代码

 s = sin(att); c = cos(att);
    si = s(1); sj = s(2); sk = s(3); 
    ci = c(1); cj = c(2); ck = c(3);
    Cnb = [ cj*ck-si*sj*sk, -ci*sk,  sj*ck+si*cj*sk;
            cj*sk+si*sj*ck,  ci*ck,  sj*sk-si*cj*ck;
           -ci*sj,           si,     ci*cj           ];
    if nargout==2  % dual Euler angle DCM
        Cnbr = [ cj*ck, si*sj*ck-ci*sk, ci*sj*ck+si*sk;
                 cj*sk, si*sj*sk+ci*ck, ci*sj*sk-si*ck;
                -sj,    si*cj,          ci*cj            ];
    end

四元数转方向余弦矩阵

公式

代码

function [qnb, att, Cnb] = attsyn(attForm)
% Attitude synchronization, i.e. let qnb, att and Cnb to represent the 
% same attitude value.
% 
% Prototype: [qnb, att, Cnb] = attsyn(attForm)
% Input: attForm - attitude in Euler angles, DCM or quaternion form
% Outputs: qnb - attitude quaterinon
%          att - Euler angles, att=[pitch; roll; yaw] in arcdeg
%          Cnb - DCM from body-frame to navigation-frame
%
 q11 = qnb(1)*qnb(1); q12 = qnb(1)*qnb(2); q13 = qnb(1)*qnb(3); q14 = qnb(1)*qnb(4); 
        q22 = qnb(2)*qnb(2); q23 = qnb(2)*qnb(3); q24 = qnb(2)*qnb(4);     
        q33 = qnb(3)*qnb(3); q34 = qnb(3)*qnb(4);  
        q44 = qnb(4)*qnb(4);
        Cnb=zeros(3,3);
        Cnb(1)=q11+q22-q33-q44;  Cnb(4)=2*(q23-q14);     Cnb(7)=2*(q24+q13);
        Cnb(2)=2*(q23+q14);      Cnb(5)=q11-q22+q33-q44; Cnb(8)=2*(q34-q12);
        Cnb(3)=2*(q24-q13);      Cnb(6)=2*(q34+q12);     Cnb(9)=q11-q22-q33+q44;

方向余弦矩阵转欧拉角

公式

代码

Convert direction cosine matrix(DCM) to Euler attitude angles.
 s = sin(att); c = cos(att);
    si = s(1); sj = s(2); sk = s(3); 
    ci = c(1); cj = c(2); ck = c(3);
    Cnb = [ cj*ck-si*sj*sk, -ci*sk,  sj*ck+si*cj*sk;
            cj*sk+si*sj*ck,  ci*ck,  sj*sk-si*cj*ck;
           -ci*sj,           si,     ci*cj           ];

方向余弦矩阵转四元数

公式

代码

C11 = Cnb(1,1); C12 = Cnb(1,2); C13 = Cnb(1,3); 
    C21 = Cnb(2,1); C22 = Cnb(2,2); C23 = Cnb(2,3); 
    C31 = Cnb(3,1); C32 = Cnb(3,2); C33 = Cnb(3,3); 
%     q0t = 0.5*sqrt(1+C11+C22+C33);
%     q1t = 0.5*sqrt(1+C11-C22-C33);
%     q2t = 0.5*sqrt(1-C11+C22-C33);
%     q3t = 0.5*sqrt(1-C11-C22+C33);
    if C11>=C22+C33
        q1 = 0.5*sqrt(1+C11-C22-C33);  qq4 = 4*q1;
        q0 = (C32-C23)/qq4; q2 = (C12+C21)/qq4; q3 = (C13+C31)/qq4;
    elseif C22>=C11+C33
        q2 = 0.5*sqrt(1-C11+C22-C33);  qq4 = 4*q2;
        q0 = (C13-C31)/qq4; q1 = (C12+C21)/qq4; q3 = (C23+C32)/qq4;
    elseif C33>=C11+C22
        q3 = 0.5*sqrt(1-C11-C22+C33);  qq4 = 4*q3;
        q0 = (C21-C12)/qq4; q1 = (C13+C31)/qq4; q2 = (C23+C32)/qq4;
    else
        q0 = 0.5*sqrt(1+C11+C22+C33);  qq4 = 4*q0;
        q1 = (C32-C23)/qq4; q2 = (C13-C31)/qq4; q3 = (C21-C12)/qq4;
    end
    qnb = [q0; q1; q2; q3];