function [] = plotFigure(t,r,v,Ib)
%PLOTFIGURE 此处显示有关此函数的摘要
%   此处显示详细说明

% 绘制上升轨迹
figure
plot3(r(:,1),r(:,2),r(:,3))
ylim([6.2e6 6.6e6])
title('发射惯性坐标系下的上升轨迹')
xlabel('x/m')
ylabel('y/m')
zlabel('z/m')
grid on

% 绘制速度
normV = sqrt(sum(v.*v,2));
figure
plot(t, normV)
title('火箭速度随时间变化曲线')
xlabel('t/s')
ylabel('v/(km/s)')
grid on

% 姿态角
[vphi,psi] = conAng(Ib);
vphi = vphi*180/pi;   % 单位转换为角度
psi = psi*180/pi;     % 单位转换为角度
figure
plot(t, vphi)
title('火箭俯仰角随时间变化曲线')
xlabel('t/s')
ylabel('\phi /deg')
grid on

figure
plot(t, psi)
title('火箭偏航角随时间变化曲线')
xlabel('t/s')
ylabel('\psi/deg')
grid on
end

function [vphi,psi] = conAng(Ib)
% 将入轨点轨道坐标系中表示的体轴矢量转换到发射惯性坐标系

n = size(Ib,1);
vphi = nan(n,1);
psi = nan(n,1);

for i = 1:n
    psi(i) = asin(-Ib(i,3));
    sinPhi = Ib(i,2)/cos(psi(i));
    if sinPhi >= 0
        vphi(i) = acos(Ib(i,1)/cos(psi(i)));
    else
        vphi(i) = -acos(Ib(i,1)/cos(psi(i)));
    end
end

end