提示:代码只给出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];