%***************************************************************************************
%**********************加上x4,x5,x6后勻速運動下的常微分方程曲線圖***********************
%***************************************************************************************

clc
clear all

[t,y]=ode45('observability2a',[0:2:35*3600],[3*pi/(60*180);3*pi/(60*180);5*pi/(60*180);5*pi/(60*180);5*pi/(60*180);6*pi/(60*180);0.1;0.1;2*pi/(60*180)]);

yy=zeros(501,9);

%每隔126點保存一次，放在矩陣yy

yy(1,:)=y(1,:);

for j=1:500
    
    for i=2:127
        
        yy(j+1,:)=y(126*j+1,:);
        
    end
    
end   

x1=yy(:,1)*180*60/pi;
x2=yy(:,2)*180*60/pi;
x3=yy(:,3)*180*60/pi;
x4=yy(:,4)*180*60/pi;
x5=yy(:,5)*180*60/pi;
x6=yy(:,6)*180*60/pi;
x7=yy(:,7);
x8=yy(:,8);
x9=yy(:,9)*180*60/pi;

t=0:252/3600:35;

%艦體姿態誤差角

figure(1)

subplot(3,1,1)
plot(t,x1,'b');
xlabel('time/h');
grid on;

subplot(3,1,2)
plot(t,x2,'b');
xlabel('time/h');
grid on;

subplot(3,1,3)
plot(t,x3,'b');
xlabel('time/h');
grid on;

%彈體誤差角

figure(2)

subplot(3,1,1)
plot(t,x4,'b');
xlabel('time/h');
grid on;

subplot(3,1,2)
plot(t,x5,'b');
xlabel('time/h');
grid on;

subplot(3,1,3)
plot(t,x6,'b');
xlabel('time/h');
grid on;

figure(3)

%速度誤差

subplot(3,1,1)
plot(t,x7,'b');
xlabel('time/h');
grid on;

subplot(3,1,2)
plot(t,x8,'b');
xlabel('time/h');
grid on;

%經度誤差角

subplot(3,1,3)
plot(t,x9,'b');
xlabel('time/h');
grid on;