四元数解算欧拉角MATLAB程序

利用公式9.2.41计算欧拉角

第一张图片是姿态矩阵，选取北东地为参考坐标系，右前下为导航坐标系，推导过程见《惯性导航》第三版绪论部分，书中选的参考系与此相反，推导出来的和图片不一样需要注意一下（三个旋转矩阵自己推导一下才能理解透彻）

第二张是姿态矩阵四元数表示推导过程见下图，将（2）式带入（1）式可得四元数矩阵

下面是程序

q=[ 1.0000         0         0         0   %上一步获取的四元数
    1.0000    0.0732         0   -0.0732
    1.0054    0.4305   -0.1811    0.3573
    1.0325    0.5766    0.1358    0.2654
    0.9052    0.8790   -0.5389    0.4492
    0.7429    0.7913   -0.8419    0.5843
    0.3204    0.8950   -1.1053    0.6016
   -0.1536    0.8097   -1.2539    0.7079
   -0.7390    0.5681   -1.1711    1.0039
   -0.9797   -0.2706   -0.9681    1.5381
   -1.1392   -0.9866    0.5532    2.2681];
N=11;
r=zeros(1,N);%横滚角初始化
p=zeros(1,N);%俯仰角初始化
y=zeros(1,N);%航向角初始化
for i=1:N
    y(1,i)=atan(2*(q(i,2)*q(i,3)+q(i,1)*q(i,4))/(q(i,1)*q(i,1)+q(i,2)*q(i,2)-q(i,3)*q(i,3)-q(i,4)*q(i,4)));
    p(1,i)=asin(2*(q(i,1)*q(i,3)-q(i,2)*q(i,4)));
    r(1,i)=atan(2*(q(i,3)*q(i,4)+q(i,1)*q(i,2))/(q(i,1)*q(i,1)-q(i,2)*q(i,2)-q(i,3)*q(i,3)+q(i,4)*q(i,4)));

end
 figure
 hold on;box on;
 plot( y, 'k-')
 plot( p,'-b.')
 plot( r,'-r.')
 legend('航向角','俯仰角','横滚角');
xlabel('t')
ylabel('y')

有问题的地方还请指出，多谢

注：图片来自秦永元老师的《惯性导航》第三版

第一张图片的推导过程看下面链接北东地/东北天两种导航坐标系与姿态转换_zht2370201的博客-CSDN博客_东北天