function h_attitudematrix1017()
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);
%%%%%
%%采用姿態匹配方法succeed！

%%%%%

%%%%%%%%%%%

%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;

    mcc11=c11;
    mcc12=c12;
    mcc13=c13;
    mcc21=c21;
    mcc22=c22;
    mcc23=c23;
    mcc31=c31;
    mcc32=c32;
    mcc33=c33;
    
rem=origratee;
rnm=origraten;
rum=origrateu;

pitchm=pitch;
rollm=roll;
yawm=yaw;

load data_s;
voutse=vout_e;
voutsn=vout_n;

pitchs=pitch;
rolls=roll;
yaws=yaw;

T=0.1;

% 
 I10=eye(10);
 I5=eye(5);
 I8=eye(8);
 I6=eye(6);
% 
%  H=[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];
%  
% 
% %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.5*pi/180*0.5*pi/180, 0.5*pi/180*0.5*pi/180, 1*pi/180*1*pi/180,0.5*pi/180*0.5*pi/180, 0.5*pi/180*0.5*pi/180, 1*pi/180*1*pi/180]);
%P0=diag([1000,1000,1000,1000,1000,1000,1000,1000]);
Z=[0;0;0];
% 
R=diag([3/60*pi/180*3/60*pi/180,3/60*pi/180*3/60*pi/180,3/60*pi/180*3/60*pi/180]);%[0.00001*g*0.00001*g,0;0,0.00001*g*0.00001*g];
% 
Q=diag([0.005*pi/180/3600*0.005*pi/180/3600,0.005*pi/180/3600*0.005*pi/180/3600,0.005*pi/180/3600*0.005*pi/180/3600,0,0,0]);
% 
X0=[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);
    
    sc=[cc11,cc12,cc13;
        cc21,cc22,cc23;
        cc31,cc32,cc33];
    mc=[mcc11(i),mcc12(i),mcc13(i);
        mcc21(i),mcc22(i),mcc23(i);
        mcc31(i),mcc32(i),mcc33(i)];
    msc=mc'*sc;
    
    
    faii=fai(i);
    
    em=rem(i);
    nm=rnm(i);
    um=rum(i);
    
    Vx0=voutse(i);
    
    
    Z(1)=msc(2,3);
    Z(2)=-msc(1,3);
    Z(3)=msc(1,2);
    
    A=[     0, um, -nm, 0,  -um, nm;
           -um,  0, em, um, 0, -em;
            nm, -em,  0, -nm, em, 0;
            0,0,0,0,0,0;
            0,0,0,0,0,0;
            0,0,0,0,0,0;
             
    ];
    
   H=[1,0,0,0,0,0; 
      0,1,0,0,0,0;
      0,0,1,0,0,0];
    
    F=I6+A*T+0.5*A*A*T*T;
    
    P1=F*P0*F'+Q;
    K=P1*H'*inv(H*P1*H'+R);
    X1=F*X0;
    X=X1+K*(Z-H*X1);
    P=(I6-K*H)*P1;
    P0=P;
    X0=X;
    
    
    r1(i)=X(1);
    r2(i)=X(2);
    r3(i)=X(3);
    
    result1(i)=P(4,4);
    result2(i)=P(5,5);
    result3(i)=P(6,6);
    %result4(i)=X(6);
    
    d1(i)=X(4);
    d2(i)=X(5);
    d3(i)=X(6);
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=d1*180/pi*60;
d2=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);