利用公式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博客_东北天