function Cnb = q2mat(qnb) % Convert attitude quaternion to direction cosine matrix(DCM). % 四元数 -> 姿态阵 % Prototype: Cnb = q2mat(qnb) % Input: qnb - attitude quaternion % Output: 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 = [ 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 ];

这是一个将四元数转换为方向余弦矩阵（DCM）的函数。它将四元数作为输入，并返回从机体坐标系到导航坐标系的DCM。

函数的实现如下：

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 ];

其中，`qnb`是输入的四元数，`Cnb`是输出的方向余弦矩阵。