% 3階調平 Alignment (經典調平)  
 
Rad_D = 0.01745329;  %Rad_Degree 
g = 9.807;
R = 6378393;
wie = 7.2921158e-5;  %弧度/秒
L = 45.7796*Rad_D;
phiz = 5*Rad_D;

phix = 0.5*Rad_D;
phiy = 0.5*Rad_D;

piaox = 0.1*Rad_D/3600;
piaoy = 0.1*Rad_D/3600;

dvx = 0.0;
dvy = 0.0;

wcx = 0.0;
wcy = 0.0;

%K11 = 0.0542;
%K21 = 1270.8;
%K31 = 15.3;

%K11 = 0.271; % 參數有問題
%K21 = 187.5;
%K31 = 0.746;



alpha(1) = phix;%初始值
beta(1) = phiy;
delvx(1) = dvx;
delvy(1) = dvy;
T = 1;         %仿真步長(秒)
step = 2400;
for i =1:step/T   
   
   
 if i<180
  K11 = 0.0542;
  K21 = 1270.8;
  K31 = 15.3;
  
   else
   K11 = 0.271;
   K21 = 187.5;
   K31 = 0.746;
  end
   
f_dvx = -phiy*g - K11*dvx;
f_dvy = phix*g - K11*dvy;

dvx = dvx + f_dvx*T;  %等價于離散化
dvy = dvy + f_dvy*T;

f_wcx = -((1+K21)*f_dvy/R + K31*dvy/R);
f_wcy = (1+K21)*f_dvx/R + K31*dvx/R;

wcx = wcx + f_wcx*T;
wcy = wcy + f_wcy*T;

%wcx = -(1+K21)*dvy/R; %二階調平不加積分環節
%wcy = (1+K21)*dvx/R;



f_phix = -wie*cos(L)*phiz + wcx - piaox;
f_phiy =  wcy - piaoy;

phix = phix + f_phix*T;
phiy = phiy + f_phiy*T;

alpha(i+1) = phix;
beta(i+1) = phiy;

delvx(i+1) = dvx;
delvy(i+1) = dvy;

end

i=1:1:step/T;
 subplot(3,2,1)
  plot(i/60,alpha(i)/Rad_D)
  subplot(3,2,2)
  plot(i/60,beta(i)/Rad_D)
  subplot(3,2,3)
  plot(i/60,delvx(i),'r')
  subplot(3,2,4)
  plot(i/60,delvy(i),'r')