%function h_anglerate()
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 ;



%由于安裝誤差角和姿態(tài)誤差角的耦合影響，所以，是難以簡(jiǎn)單的區(qū)分的。

 %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=ratee;
rnm=raten;
rum=rateu;

origrem=origratee;
origrnm=origraten;
origrum=origrateu;

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

load data_s;
voutse=vout_e;
voutsn=vout_n;

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

origres=origratee;
origrns=origraten;
origrus=origrateu;


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.05*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
    
    fe=aout_e(i);
    fn=aout_n(i);
    fu=aout_u(i);
    
    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=origrem(i);
    nm=origrnm(i);
    um=origrum(i);
    
    Z(1)=-origrem(i)+origres(i);
    Z(2)=-origrnm(i)+origrns(i);
    Z(3)=-origrum(i)+origrus(i);
    
    Vx0=voutse(i);
    
    
    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=[0,0,0,0,um,-nm; 
      0,0,0,-um,0,em;
      0,0,0,nm,-em,0];
   %Z=H*X0;
    
    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('時(shí)間(s)');
% ylabel('失調(diào)角估計(jì)誤差(度)');
% %title('設(shè)定值為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('失調(diào)角估計(jì)(度)');
% xlabel('時(shí)間(s)×0.1');
% %title('設(shè)定值為1度');


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

plot(d1);
hold on;
plot(d2,':');
hold on;
plot(d3,'-.');
ylabel('不對(duì)準(zhǔn)角估計(jì)(分)');
xlabel('時(shí)間(s)×0.1');
%title('設(shè)定值為1度');


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