%% 画图程序_math情况
%作者：范黎明
%时间：2019年3月26日

clc;
close all;
L=1; %线宽LineWidth
ubound=[0.3;0.3;0.2];

%% 画第1个图：误差欧拉角
figure(1)
plot(Fig1errorEuler(1:2001,1),Fig1errorEuler(1:2001,2),'r--','LineWidth',L);
hold on;
plot(Fig1errorEuler(1:2001,1),Fig1errorEuler(1:2001,3),'b','LineWidth',L);
hold on;
plot(Fig1errorEuler(1:2001,1),Fig1errorEuler(1:2001,4),'g-.','LineWidth',L);
hold on;
grid on;
axis([0,20,-22,12]);
xlabel('Time (s)');
ylabel('Error Euler Angle (°)');
legend('Error Euler Angle in Roll axis','Error Euler Angle in Pitch axis','Error Euler Angle in Yaw axis');
set(gcf,'position',[1700,500,600,300]);
% 补充小图
axes('position',[0.5,0.3,0.3,0.2]);
plot(Fig1errorEuler(1401:1801,1),Fig1errorEuler(1401:1801,2),'r--','LineWidth',L);
hold on;
plot(Fig1errorEuler(1401:1801,1),Fig1errorEuler(1401:1801,3),'b','LineWidth',L);
hold on;
plot(Fig1errorEuler(1401:1801,1),Fig1errorEuler(1401:1801,4),'g-.','LineWidth',L);
hold on;

%% 画第2个图：误差欧拉角速度
figure(2)
plot(Fig2errorEw(1:2001,1),Fig2errorEw(1:2001,2),'r--','LineWidth',L);
hold on;
plot(Fig2errorEw(1:2001,1),Fig2errorEw(1:2001,3),'b','LineWidth',L);
hold on;
plot(Fig2errorEw(1:2001,1),Fig2errorEw(1:2001,4),'g-.','LineWidth',L);
hold on;
grid on;
axis([0,20,-7,11]);
xlabel('Time (s)');
ylabel('Error Angular Velocity (°/s)');
legend('Error Angular Velocity in Roll axis','Error Angular Velocity in Pitch axis','Error Angular Velocity in Yaw axis');
set(gcf,'position',[2500,500,600,300]);
% 补充小图
axes('position',[0.5,0.6,0.3,0.2]);
plot(Fig2errorEw(1401:1801,1),Fig2errorEw(1401:1801,2),'r--','LineWidth',L);
hold on;
plot(Fig2errorEw(1401:1801,1),Fig2errorEw(1401:1801,3),'b','LineWidth',L);
hold on;
plot(Fig2errorEw(1401:1801,1),Fig2errorEw(1401:1801,4),'g-.','LineWidth',L);
hold on;

%% 画第3个图：观测器有限时间
figure(3)
plot(Fig3finitetime(1:21,1),Fig3finitetime(1:21,2),'r--','LineWidth',L);
hold on;
plot(Fig3finitetime(1:21,1),Fig3finitetime(1:21,3),'b','LineWidth',L);
hold on;
plot(Fig3finitetime(1:21,1),Fig3finitetime(1:21,4),'g-.','LineWidth',L);
hold on;
grid on;
axis([0,0.2,-0.08,0.07]);
hold on;
ylim=get(gca,'Ylim');
hold on;
plot([0.1,0.1],ylim,'k--','LineWidth',L);
xlabel('Time (s)');
ylabel('Observation Error ');
legend('e_1','e_2','e_3','t=0.1s');
set(gcf,'position',[2500,100,600,300]);

%% 画第4个图：MPD变参数
figure(4)
plot(Fig4beita(:,1),Fig4beita(:,2),'r--','LineWidth',L);
hold on;
plot(Fig4beita(:,1),Fig4beita(:,3),'b','LineWidth',L);
hold on;
plot(Fig4beita(:,1),Fig4beita(:,4),'g-.','LineWidth',L);
hold on;
grid on;
axis([0,200,-0.1,1.1]);
xlabel('Time (s)');
ylabel('Modified Gain \beta ');
set(gcf,'position',[1700,100,600,300]);
% 补充小图
axes('position',[0.25,0.25,0.6,0.5]);
plot(Fig4beita(1:201,1),Fig4beita(1:201,2),'r--','LineWidth',L);
hold on;
plot(Fig4beita(1:201,1),Fig4beita(1:201,3),'b','LineWidth',L);
hold on;
plot(Fig4beita(1:201,1),Fig4beita(1:201,4),'g-.','LineWidth',L);
hold on;
axis([0,2,-0.1,1.1]);
hold on;
ylim=get(gca,'Ylim');
hold on;
plot([0.1,0.1],ylim,'k--','LineWidth',L);
hold on;
legend('\beta_1','\beta_2','\beta_3','t=0.1s');
hold on;

%% 画第5个图：假设条件状态
figure(5)
% 子图1
subplot(3,1,1);
plot(Fig5assumption(:,1),Fig5assumption(:,2),'r','LineWidth',L);
hold on;
plot([0,Fig5assumption(20001,1)],[ubound(1),ubound(1)],'k-.','LineWidth',L);
hold on;
plot([0,Fig5assumption(20001,1)],[-ubound(1),-ubound(1)],'k-.','LineWidth',L);
hold on;
grid on;
axis([0,200,-0.35,0.35]);
xlabel('Time (s)');
ylabel('u_{NC1} (Nm)');
legend('u_{NC1}','u_{bound1}');
% 子图2
subplot(3,1,2);
plot(Fig5assumption(:,1),Fig5assumption(:,3),'b','LineWidth',L);
hold on;
plot([0,Fig5assumption(20001,1)],[ubound(2),ubound(2)],'k-.','LineWidth',L);
hold on;
plot([0,Fig5assumption(20001,1)],[-ubound(2),-ubound(2)],'k-.','LineWidth',L);
hold on;
grid on;
axis([0,200,-0.35,0.35]);
xlabel('Time (s)');
ylabel('u_{NC2} (Nm)');
legend('u_{NC2}','u_{bound2}');
% 子图3
subplot(3,1,3);
plot(Fig5assumption(:,1),Fig5assumption(:,4),'g','LineWidth',L);
hold on;
plot([0,Fig5assumption(20001,1)],[ubound(3),ubound(3)],'k-.','LineWidth',L);
hold on;
plot([0,Fig5assumption(20001,1)],[-ubound(3),-ubound(3)],'k-.','LineWidth',L);
hold on;
grid on;
axis([0,200,-0.22,0.22]);
xlabel('Time (s)');
ylabel('u_{NC3} (Nm)');
legend('u_{NC3}','u_{bound3}');
set(gcf,'position',[1800,100,600,700]);

%% 画第6个图：控制输入
figure(6)
% 子图1
subplot(3,1,1);
plot(Fig6uuuu(:,1),Fig6uuuu(:,2),'r','LineWidth',L);
hold on;
plot([0,Fig6uuuu(20001,1)],[ubound(1),ubound(1)],'k-.','LineWidth',L);
hold on;
plot([0,Fig6uuuu(20001,1)],[-ubound(1),-ubound(1)],'k-.','LineWidth',L);
hold on;
grid on;
axis([0,200,-0.35,0.35]);
xlabel('Time (s)');
ylabel('u_1 (Nm)');
legend('u_1','u_{bound1}');
% 补充小图
axes('position',[0.6,0.77,0.2,0.04]);
plot(Fig6uuuu(1:61,1),Fig6uuuu(1:61,2),'r','LineWidth',L);
hold on;
plot([0,Fig6uuuu(61,1)],[ubound(1),ubound(1)],'k-.','LineWidth',L);
hold on;
axis([0,0.6,0,0.4]);
hold on;

% 子图2
subplot(3,1,2);
plot(Fig6uuuu(:,1),Fig6uuuu(:,3),'b','LineWidth',L);
hold on;
plot([0,Fig6uuuu(20001,1)],[ubound(2),ubound(2)],'k-.','LineWidth',L);
hold on;
plot([0,Fig6uuuu(20001,1)],[-ubound(2),-ubound(2)],'k-.','LineWidth',L);
hold on;
grid on;
axis([0,200,-0.35,0.35]);
xlabel('Time (s)');
ylabel('u_2 (Nm)');
legend('u_2','u_{bound2}');
% 补充小图
axes('position',[0.6,0.47,0.2,0.04]);
plot(Fig6uuuu(1:161,1),Fig6uuuu(1:161,3),'b','LineWidth',L);
hold on;
plot([0,Fig6uuuu(161,1)],[-ubound(2),-ubound(2)],'k-.','LineWidth',L);
hold on;
axis([0,1.6,-0.4,0]);
hold on;

% 子图3
subplot(3,1,3);
plot(Fig6uuuu(:,1),Fig6uuuu(:,4),'g','LineWidth',L);
hold on;
plot([0,Fig6uuuu(20001,1)],[ubound(3),ubound(3)],'k-.','LineWidth',L);
hold on;
plot([0,Fig6uuuu(20001,1)],[-ubound(3),-ubound(3)],'k-.','LineWidth',L);
hold on;
grid on;
axis([0,200,-0.22,0.22]);
xlabel('Time (s)');
ylabel('u_3 (Nm)');
legend('u_3','u_{bound3}');
set(gcf,'position',[1900,100,600,700]);
% 补充小图
axes('position',[0.6,0.17,0.2,0.04]);
plot(Fig6uuuu(1:16,1),Fig6uuuu(1:16,4),'g','LineWidth',L);
hold on;
plot([0,Fig6uuuu(16,1)],[-ubound(3),-ubound(3)],'k-.','LineWidth',L);
hold on;
axis([0,0.15,-0.21,-0.15]);
hold on;

%% 画第7个图：挠性振动状态
figure(7)
% 子图1
subplot(3,1,1);
plot(Fig7vibrationstate(:,1),Fig7vibrationstate(:,2),'r','LineWidth',L);
hold on;
grid on;
axis([0,200,-0.6,0.5]);
xlabel('Time (s)');
ylabel('\eta_x (m)');
legend('\eta_x');
axes('position',[0.6,0.75,0.2,0.05]);
plot(Fig7vibrationstate(19001:20001,1),Fig7vibrationstate(19001:20001,2),'r','LineWidth',L);

% 子图2
subplot(3,1,2);
plot(Fig7vibrationstate(:,1),Fig7vibrationstate(:,3),'b','LineWidth',L);
hold on;
grid on;
axis([0,200,-0.4,0.45]);
xlabel('Time (s)');
ylabel('\eta_y (m)');
legend('\eta_y');
axes('position',[0.6,0.55,0.2,0.05]);
plot(Fig7vibrationstate(19001:20001,1),Fig7vibrationstate(19001:20001,3),'b','LineWidth',L);

% 子图3
subplot(3,1,3);
plot(Fig7vibrationstate(:,1),Fig7vibrationstate(:,4)*180/pi,'g','LineWidth',L);
hold on;
grid on;
axis([0,200,-15,12]);
xlabel('Time (s)');
ylabel('\eta_z (°)');
legend('\eta_z');
set(gcf,'position',[2000,100,600,700]);
axes('position',[0.6,0.15,0.2,0.05]);
plot(Fig7vibrationstate(19001:20001,1),Fig7vibrationstate(19001:20001,4)*180/pi,'g','LineWidth',L);
axis([190,200,-0.0005,-0.0001]);
