velocitymatchsimulation.m

捷联惯导系统传递对准MATLAB程序

        Tnb1=[Tnb11(1) Tnb12(1) Tnb13(1)
            Tnb21(1) Tnb22(1) Tnb23(1)
            Tnb31(1) Tnb32(1) Tnb33(1)];
        Tnb2=[Tnb11(2) Tnb12(2) Tnb13(2)
            Tnb21(2) Tnb22(2) Tnb23(2)
            Tnb31(2) Tnb32(2) Tnb33(2)];
        Tnb3=[Tnb11(3) Tnb12(3) Tnb13(3)
            Tnb21(3) Tnb22(3) Tnb23(3)
            Tnb31(3) Tnb32(3) Tnb33(3)];
        
        winb1=Tnb1*winn;
        winb2=Tnb2*winn;
        winb3=Tnb3*winn;
        
        wibb1=winb1+wnbb1;
        wibb2=winb2+wnbb2;
        wibb3=winb3+wnbb3;
        %%%%%%%%%%%%%%%%%%%%%%
        fn(1,1) = a(1) - (2*wien(3)+wenn(3))*v0(2);
        fn(2,1) = a(2) + (2*wien(3)+wenn(3))*v0(1);
        fn(3,1) = a(3) + (2*wien(1)+wenn(1))*v0(2) - (2*wien(2)+wenn(2))*v0(1) + g1;
        %%%%%%%%%%%%%%%%
        fbb1=Tnb1*fn;
        fbb2=Tnb2*fn;
        fbb3=Tnb3*fn;
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%             
        %__________---------因为假设主惯导和子惯导的载体坐标系是重合的，所以在模拟子惯导的加速度和角速度信息时
        %%%%%%%%%----直接在主惯导输出上加上子惯导的漂移%%%%%%%%
        wib1=Kg*wibb1;%+G_Drift+0.1*G_Drift*randn(1);
        wib2=Kg*wibb2;%+G_Drift+0.1*G_Drift*randn(1);
        wib3=Kg*wibb3;%+G_Drift+0.1*G_Drift*randn(1);       
        
        %     fb1=Ka*fbb1+A_Bias+0.1*A_Bias*randn(1);
        %     fb2=Ka*fbb2+A_Bias+0.1*A_Bias*randn(1);
        fb3=Ka*fbb3;%+A_Bias+0.1*A_Bias*randn(1);
        %     fprintf(fid,'%14.8f%14.8f%14.8f',fb3(1),fb3(2),fb3(3));
        
        temp=[0  -wib3(3)   wib3(2)
            wib3(3)    0     -wib3(1)
            -wib3(2)   wib3(1)   0];
        
        temp1=temp*r;
        temp2=inv(Tbn)*temp*temp1;
        
        omiga=[0  -wyT2(3)  wrT2(3)
            wyT2(3)   0     -wpT2(3)
            -wrT2(3)   wpT2(3)  0 ];
        temp3=inv(Tbn)*omiga*r;    
        
        fb3=temp2+temp3+fb3;
        out=T*(temp2+temp3);
        
        
        %      fb=T*fb3;
        %     fprintf(fid,'%14.8f%14.8f%14.8f%14.8f%14.8f%14.8f\n',fb(1),fb(2),fb(3),temp3(1),temp3(2),temp3(3)); 
        % fb3(1)=data(k,1);
        % fb3(2)=data(k,2);
        % fb3(3)=data(k,3);
        
        %%%%%%%%%%%%%%
        q0 = q;
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        wpbb1=wib1-inv(T)*(wepp+wiep);
        wpbb2=wib2-inv(T)*(wepp+wiep);
        wpbb3=wib3-inv(T)*(wepp+wiep);
        
        omiga1=[0       -wpbb1(1) -wpbb1(2) -wpbb1(3)
            wpbb1(1)  0         wpbb1(3) -wpbb1(2)
            wpbb1(2) -wpbb1(3)  0         wpbb1(1)
            wpbb1(3)  wpbb1(2) -wpbb1(1)  0       ];
        
        omiga2=[0        -wpbb2(1) -wpbb2(2) -wpbb2(3)
            wpbb2(1)     0     wpbb2(3)  -wpbb2(2)
            wpbb2(2) -wpbb2(3)  0         wpbb2(1)
            wpbb2(3)  wpbb2(2) -wpbb2(1)  0       ];
        
        omiga3=[0        -wpbb3(1) -wpbb3(2) -wpbb3(3)
            wpbb3(1)  0         wpbb3(3) -wpbb3(2)
            wpbb3(2) -wpbb3(3)  0         wpbb3(1)
            wpbb3(3)  wpbb3(2) -wpbb3(1)  0       ];
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
        k1 = 1/2*omiga1*q;
        q=q0+k1*Hn/2;
        
        k2=1/2*omiga2*q;
        q=q0+k2*Hn/2;
        
        k3=1/2*omiga2*q;
        q=q0+k3*Hn;
        
        k4=1/2*omiga3*q;
        
        q=q0+(k1+2*k2+2*k3+k4)/6*Hn;
        
        q=q/sqrt(q(1)^2+q(2)^2+q(3)^2+q(4)^2);
        
        T=[q(1)^2+q(2)^2-q(3)^2-q(4)^2  2*(q(2)*q(3)-q(1)*q(4))      2*(q(2)*q(4)+q(1)*q(3))
            2*(q(2)*q(3)+q(1)*q(4))     q(1)^2-q(2)^2+q(3)^2-q(4)^2  2*(q(3)*q(4)-q(1)*q(2))
            2*(q(2)*q(4)-q(1)*q(3))     2*(q(3)*q(4)+q(1)*q(2))      q(1)^2-q(2)^2-q(3)^2+q(4)^2];   
        
        temp=[0  -wib3(3)   wib3(2)
            wib3(3)    0     -wib3(1)
            -wib3(2)   wib3(1)   0];  
        
        
        fp=T*fb3;
        
        M_temp=[0  -wibb3(3)   wibb3(2)
            wibb3(3)    0     -wibb3(1)
            -wibb3(2)   wibb3(1)   0]; 
        
        M_temp1=M_temp*r;
        M_temp2=M_temp*M_temp1;
        
        M_omiga=[0  -wyT2(3)  wrT2(3)
            wyT2(3)   0     -wpT2(3)
            -wrT2(3)   wpT2(3)  0 ];
        M_temp3=M_omiga*r+M_temp2;
%         fp=fp-M_temp3;
        
        temp_v=M_temp*M_temp+M_omiga;
        
%         out2=M_temp3;
        
        %         fprintf(fid,'%14.8f%14.8f%14.8f%14.8f%14.8f%14.8f\n',fp(1),fp(2),fp(3),temp3(1),temp3(2),temp3(3));
        %         fp(1)=data(k,1);
        %         fp(2)=data(k,2);
        %         fp(3)=data(k,3);
        %             
        f_vx = fp(1) + (2*wiep(3)+wepp(3))*v(2);
        f_vy = fp(2) - (2*wiep(3)+wepp(3))*v(1);
        
        v(1) = v(1) + f_vx*Hn;
        v(2) = v(2) + f_vy*Hn;
        
        
        wepp = [-v(2)/Ryp;
            v(1)/Rxp;
            v(1)*tan(phi)/Rxp];
        
        phi  = phi+v(2)*Hn/Ryp;
        lamda= lamda+v(1)*Hn/(Rxp*cos(phi));    
        wiep = [0; wie*cos(phi); wie*sin(phi)];
        g1 = g0+0.051799*sin(phi)*sin(phi);
        
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        A =[0                              2*wiep(3)+wepp(3)      0                               -fp(3)                     fp(2)                     temp_v(1,1)        temp_v(1,2)   temp_v(1,3)          
            -(2*wiep(3)+wepp(3))           0                      fp(3)                           0                          -fp(1)                    temp_v(2,1)     temp_v(2,2)     temp_v(2,3)         
            0                              -1/Ryp                 0                               wiep(3)+v(1)*tan(phi)/Rxp  -(wiep(2)+wepp(2))        0             0          0    
            1/Rxp                          0                      -(wiep(3)+wepp(3))              0                          wepp(1)                   0             0          0   
            1*tan(phi)/Rxp                 0                      wiep(2)+wepp(2)                 v(2)/Ryp                   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=[ 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     0      0    
            0      0      0      0      0      0      1     0     
            0      0      0      0      0      0      0      1 ];   
        
        [F,G] = c2d(A,B,Hn);
        FT = F';
        GT = G';
        HT=H';
        z=v-v0;
        Z=[z(1)
            z(2)];
        %    Z=[z(1);
        %        z(2)];
        
        Xk = F*X;
        Zk = H*Xk;
        Pk = F*P*FT + G*Q*GT;
        K  = Pk*HT*inv(H*Pk*HT+R);
        P  = (eye(8)-K*H)*Pk;
        X  = Xk + K*(Z-Zk);
        
        datx4(k)=X(1);
        datx5(k)=X(2);
        datx1(k) = X(3)/rad_deg*60;
        datx2(k) = X(4)/rad_deg*60;
        datx3(k) = X(5)/rad_deg*60;
        
        datx6(k) = X(6);
        datx7(k) = X(7);
        datx8(k) = X(8);
%         datx9(k) = X(9)/rad_deg*3600;
%         datx10(k) = X(10)/rad_deg*3600;
        
        dvx(k)=Z(1);
        dvy(k)=Z(2);
    end
    
    % fclose(fid);
    
    
    j=1:length(datx1);
    figure(41)
    subplot(3,1,1)
    plot(j/10,(datx1(j)-dp))
    subplot(3,1,2)
    plot(j/10,(datx2(j)-dr))
    subplot(3,1,3)
    plot(j/10,(datx3(j)-dy))
    
    figure(42)
    subplot(3,1,1)
    plot(j/10,datx6(j));
    subplot(3,1,2)
    plot(j/10,datx7(j));
    subplot(3,1,3)
    plot(j/10,datx8(j));
    
    figure(3)
    subplot(2,1,1)
    plot(dvx);
    hold on;
    plot(datx4,'r');
    hold off;
    
    subplot(2,1,2)
    plot(dvy);
    hold on;
    plot(datx5,'r');
    hold off
    % subplot(5,1,4)
    % plot(j,datx9(j));
    % subplot(5,1,5)
    % plot(j,datx10(j));