捷联惯导算法程序

一、姿态形式转换

1、欧拉角->姿态阵

function Cnb = a2mat(att)
    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         ];
end

其中，i->θ（俯仰角），j->γ（滚转角），k->ψ（航向角）。

2、欧拉角->四元数

function qnb = a2qua(att)
    s = sin(att/2); c = cos(att/2);
    si = s(1); sj = s(2); sk = s(3); ci = c(1); cj = c(2); ck = c(3); 
    qnb = [ ci*cj*ck - si*sj*sk;
          si*cj*ck - ci*sj*sk;
          ci*sj*ck + si*cj*sk;
          ci*cj*sk + si*sj*ck ];
    % qnb = m2qua(a2mat(att));
end

 3、姿态阵->欧拉角

function att = m2att(Cnb)
    if abs(Cnb(3,2))<=0.999999
        att = [ asin(Cnb(3,2)); -atan2(Cnb(3,1),Cnb(3,3)); -atan2(Cnb(1,2),Cnb(2,2)) ];
    else
        att = [ asin(Cnb(3,2));  atan2(Cnb(1,3),Cnb(1,1));  0 ];
    end
end

4、四元数->姿态阵

function Cnb = q2mat(qnb)
    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 = [ q11+q22-q33-q44,  2*(q23-q14),      2*(q24+q13);
           2*(q23+q14),     q11-q22+q33-q44,  2*(q34-q12);
           2*(q24-q13),      2*(q34+q12),     q11-q22-q33+q44 ];
end

 

 5、姿态阵->四元数

function qnb = m2qua(Cnb)
    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); 
    if C11>=C22+C33
        q1 = 0.5*sqrt(1+C11-C22-C33);
        q0 = (C32-C23)/(4*q1); q2 = (C12+C21)/(4*q1); q3 = (C13+C31)/(4*q1);
    elseif C22>=C11+C33
        q2 = 0.5*sqrt(1-C11+C22-C33);
        q0 = (C13-C31)/(4*q2); q1 = (C12+C21)/(4*q2); q3 = (C23+C32)/(4*q2);
    elseif C33>=C11+C22
        q3 = 0.5*sqrt(1-C11-C22+C33);
        q0 = (C21-C12)/(4*q3); q1 = (C13+C31)/(4*q3); q2 = (C23+C32)/(4*q3);
    else
        q0 = 0.5*sqrt(1+C11+C22+C33);
        q1 = (C32-C23)/(4*q0); q2 = (C13-C31)/(4*q0); q3 = (C21-C12)/(4*q0);
    end
    qnb = [q0; q1; q2; q3];
end

 

 6、四元数->欧拉角

function att = q2att(qnb)
    att = m2att(q2mat(qnb));
end

小小的偷懒，实际转换关系如下：

以上则为欧拉角、姿态阵、四元数两两之间的转换。

参考：西北工业大学严恭敏《捷联惯导算法与组合导航原理》