%function h_speedposition()
clc;
%clear;
%Ts, maxcount, ae, an au, p_m, r_m, y_m, fp, fr, fy, 
%nav_data_m_fuc(0.01,2*600,  0,0,0,  0,0,0,  0,0,0   );

%nav_data_s_fuc(1,1,1);


%%%%%%%%%%%

%transfer alignment with my own method and dymamic equation ;



%由于安裝誤差角和姿態誤差角的耦合影響，所以，是難以簡單的區分的。

 %maxcount=20*60;
 
 o_miga=360.0/24*pi/180/3600;
 
 %fai=45*pi/180;
 
 %Ts=1;
 
 RR=6378137.0;
 
 g=9.8;
  
 Vx0=0;
 Vy0=0;
 
 
load data_m;
voutme=vout_e;
voutmn=vout_n;

rem=ratee;
rnm=raten;
rum=rateu;

faim=fai;
lm=l;


load data_s;
voutse=vout_e;
voutsn=vout_n;

fais=fai;
ls=l;

T=0.1;

% 
 I10=eye(10);
 I5=eye(5);
 I8=eye(8);
% 
 H=[1,0,0,0,0,0,0,0,0,0; 
    0,1,0,0,0,0,0,0,0,0;
    0,0,1,0,0,0,0,0,0,0;
    0,0,0,1,0,0,0,0,0,0];
%  
% 
% %P0=diag([1*1,1*1,0.5*pi/180*0.5*pi/180, 0.5*pi/180*0.5*pi/180, pi/180*pi/180,1*pi/180*pi/180, 1*pi/180*pi/180, 2*pi/180*2*pi/180]);%, 0.0001*g*0.0001*g, 0.0001*g*0.0001*g, 0.02*pi/180/3600*0.02*pi/180/3600, 0.02*pi/180/3600*0.02*pi/180/3600, 0.02*pi/180/3600*0.02*pi/180/3600]);
P0=diag([0.1*0.1,0.1*0.1,0.1*0.1,0.1*0.1,1*pi/180*1*pi/180, 1*pi/180*1*pi/180, 2*pi/180*2*pi/180,1*pi/180*1*pi/180, 1*pi/180*1*pi/180, 2*pi/180*2*pi/180]);
%P0=diag([0.1*0.1,0.1*0.1,0.1*0.1,0.1*0.1,0.1*0.1,0.1*0.1,0.1*0.1,0.1*0.1]);
Z=[0;0;0;0];
% 
R=diag([0.1*0.1,0.1*0.1,0.1*0.1,0.1*0.1]);%[0.00001*g*0.00001*g,0;0,0.00001*g*0.00001*g];
% 
Q=diag([0.001*0.001,0.001*0.001,0.001*g*0.001*g,0.001*g*0.001*g,0.1*pi/180/3600*0.1*pi/180/3600,0.1*pi/180/3600*0.1*pi/180/3600,0.1*pi/180/3600*0.1*pi/180/3600,0.00*pi/180,0.00*pi/180,0.00*pi/180]);
% 

X0=[0;0;0;0;0;0;0;0;0;0];


for i=1:maxcount*Ts*10
    
if manner==4
    fe=0;%0.05*vout_n(i)*sin(2.0*pi*i/100);
    fn=0.1*vout_n(i)*sin(2.0*pi*i/100);
    fu=aout_u(i);
else

    fe=aout_e(i);
    fn=aout_n(i);
    fu=aout_u(i);
end    
    cc11=c11(i);
    cc12=c12(i);
    cc13=c13(i);
    cc21=c21(i);
    cc22=c22(i);
    cc23=c23(i);
    cc31=c31(i);
    cc32=c32(i);
    cc33=c33(i);
    
    faii=fai(i);
    
    em=rem(i);
    nm=rnm(i);
    um=rum(i);
    
    Vx0=voutse(i);
    
    Z(1)=faim(i)-fais(i);
    Z(2)=lm(i)-ls(i);
    
    Z(3)=-voutse(i)+voutme(i);
    Z(4)=-voutsn(i)+voutmn(i);
    
    
  A=[        0,  0,            0,                          1/RR,       0,0,0,0,0,0;
      Vx0/RR*sec(faii)*tan(faii),0,sec(faii)/RR ,               0,          0,0,0,0,0,0;
             0,0,                0,                           (1.0*o_miga*sin(faii)+Vx0/RR*tan(faii)),           cc13*fn-cc12*fu,    -cc13*fe+cc11*fu ,  cc12*fe-cc11*fn,   -cc13*fn+cc12*fu,   cc13*fe-cc11*fu,  -cc12*fe+cc11*fn;
             0,0,       (-1.0*o_miga*sin(faii)-Vx0/RR*tan(faii)),                   0,                            cc23*fn-cc22*fu,    -cc23*fe+cc21*fu,   cc22*fe-cc21*fn,   -cc23*fn+cc22*fu,   cc23*fe-cc21*fu,   -cc22*fe+cc21*fn;
             0,0,                0,           0,                                                                0, um, -nm, 0,  -um, nm;
             0,0,                0,           0,                                                               -um,  0, em, um, 0, -em;
             0,0,                0,           0,                                                               nm, -em,  0, -nm, em, 0;
             0,0,                0,           0,                                                               0,0,0,0,0,0;
             0,0,                0,           0,                                                               0,0,0,0,0,0;
             0,0,                0,           0,                                                               0,0,0,0,0,0;
             
    ];
    
%     B=[cc11,cc12, 0,0,0,0,0,0;
%        cc21,cc22, 0,0,0,0,0,0;
%         0,    0,  1,0,0,0,0,0;
%         0,    0,  0,1,0,0,0,0;
%         0,    0,  0,0,1,0,0,0;
%         0,    0,  0,0,0,1,0,0;
%         0,    0,  0,0,0,0,1,0;
%         0,    0,  0,0,0,0,0,1];
%     Q1=B*Q0*B';
    
    F=I10+A*T+0.5*A*A*T*T;
    
%     Q=(Q1+F*Q1*F')*T/2;
   
    P1=F*P0*F'+Q;
    K=P1*H'*inv(H*P1*H'+R);
    X1=F*X0;
    X=X1+K*(Z-H*X1);
    P=(I10-K*H)*P1;
    P0=P;
    X0=X;
    
    
    r1(i)=X(3);
    r2(i)=X(4);
    r3(i)=X(5);
    
    result1(i)=P(3,3);
    result2(i)=P(4,4);
    result3(i)=P(5,5);
    %result4(i)=X(6);
    
    d1(i)=X(8);
    d2(i)=X(9);
    d3(i)=X(10);
end

result1=sqrt(result1)*180/pi;
result2=sqrt(result2)*180/pi;
result3=sqrt(result3)*180/pi;
%result4=result4*10000/g;
%result4(maxcount)

% figure;
% plot(result1,'g');
% hold on;
% plot(result2,'r');
% hold on;
% plot(result3);
% xlabel('時間(s)');
% ylabel('失調角估計誤差(度)');
% %title('設定值為1度');

% 
% r1=r1*180/pi;
% r2=r2*180/pi;
% r3=r3*180/pi;
% figure;
% 
% plot(r1,'g');
% hold on;
% plot(r2,'r');
% hold on;
% plot(r3);
% ylabel('失調角估計(度)');
% xlabel('時間(s)×0.1');
% %title('設定值為1度');


d1=2*d1*180/pi*60;
d2=2*d2*180/pi*60;
d3=d3*180/pi*60;

figure;

x=0.1:0.1:60;



plot(x,d1);
hold on;
plot(x,d2,':');
hold on;
plot(x,d3,'-.');
ylabel('不對準角估計(分)');
xlabel('時間(s)×0.1');
%title('設定值為1度');

d1
d2
d3


%figure;
%plot(velocity_x);
%hold on;
%plot(velocity_y);