zdzcx.m

一个惯性导航子对准程序

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 惯性导航系统
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc;
clear all;
close all;
load mwains.txt;
load gps1.dat;
format long
fid10 = fopen('mcszdz154.txt','w+');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
kkkk=3000;
for i=1:kkkk
    mwains0(i,:)=mean(mwains(i:i+100,:));
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%地球自转角速率、地球半径、曲率半径、经度、纬度、高度、重力加速度
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Wie=0.000072722;Rr=6378245;e=1/298.3;g0=9.7803267714;g00=9.7803267714;
Tins=0.010;
mvn=0;mvu=0;mve=0;mL=0;mh=0;mJ=0;
mp=0;mr=0;my=-(0+0/60+0/3600)/(180/pi);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
g0=g00*(1+5.27094e-3*sin(mL)^2+2.32718e-5*sin(mL)^4)-3.086e-6*mh;
Rn=(1-2*e+3*e*sin(mL)^2)*Rr;
Re=(1+e*sin(mL)^2)*Rr;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x00=[mr my mp];
mx00=x00;
mq=[cos(x00(3)/2.0)*cos(x00(2)/2.0)*cos(x00(1)/2.0)-sin(x00(3)/2.0)*sin(x00(2)/2.0)*sin(x00(1)/2.0);
   cos(x00(3)/2.0)*cos(x00(2)/2.0)*sin(x00(1)/2.0)+sin(x00(3)/2.0)*sin(x00(2)/2.0)*cos(x00(1)/2.0);
   cos(x00(3)/2.0)*sin(x00(2)/2.0)*cos(x00(1)/2.0)+sin(x00(3)/2.0)*cos(x00(2)/2.0)*sin(x00(1)/2.0);
   sin(x00(3)/2.0)*cos(x00(2)/2.0)*cos(x00(1)/2.0)-cos(x00(3)/2.0)*sin(x00(2)/2.0)*sin(x00(1)/2.0);]; 
mCbn=[mq(1)^2+mq(2)^2-mq(3)^2-mq(4)^2,2*(mq(2)*mq(3)-mq(1)*mq(4)),2*(mq(2)*mq(4)+mq(1)*mq(3));
      2*(mq(2)*mq(3)+mq(1)*mq(4)),mq(1)^2-mq(2)^2+mq(3)^2-mq(4)^2,2*(mq(3)*mq(4)-mq(1)*mq(2));
      2*(mq(2)*mq(4)-mq(1)*mq(3)),2*(mq(3)*mq(4)+mq(1)*mq(2)),mq(1)^2-mq(2)^2-mq(3)^2+mq(4)^2];
mCbn11=mCbn;  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%初始对准
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%系统状态误差初始值设置
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
efins=[0.5;0.5;0.5];             % 姿态角初始误差
evins=[0.1;0.1;0.1;];            % 惯导速度初始误差
epins=[100/Rr;100/Rr;100;];      % 惯导位置初始误差
eg=[0.1;0.1;0.1]/57.3;           % 陀螺漂移误差
ea=[0.01;0.01;0.01;]*g0;         % 加计常值漂移误差
ex0=[efins;evins;epins;];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 系统方程白噪声均方差
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
wfinsm=[std(mwains(1:2000,1:3))]'*0.01*2;
wvinsm=[std(mwains(1:2000,4:6))]'*g0*0.01*2;
wpinsm=[10/Rr;10/Rr;10;];
wgm=[std(mwains(1:2000,1:3))]'; 
wam=[std(mwains(1:2000,4:6))]'*g0;
wwm=[wfinsm;wvinsm;wpinsm;];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
v=[[std(mwains(1:2000,4:6))]'*g0*0.01*2;];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ww3=[wwm(1:9);];
x03=[ex0(1:9);];
v3=v;
mQt3=diag(ww3.^2);     
mRk3=diag(v3.^2);    
mPk3=diag(x03.^2);    
mXk3=zeros(9,1);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
mvins=[0 0 0]; mpins=[mL mJ mh];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for k=1:kkkk/2
    mwains1=mwains0(2*k-1,1:6);
    mwains2=mwains0(2*k,1:6);
    mWiex=Wie*cos(mL);
    mWiey=Wie*sin(mL);
    mWiez=0;
    mWinb=mCbn11'*[mWiex;mWiey;mWiez;];
    mWnb1=mwains1(1:3)-mWinb';
    mWnb2=mwains2(1:3)-mWinb';
    mwebb(2*k-1,1:3)=mWnb1;
    mwebb(2*k,1:3)=mWnb2;
    mt1=mWnb1*Tins/2;
    mt2=mWnb2*Tins/2;
    mst=mt1+mt2+2/3*cross(mt1,mt2);
    mita=[0 -mst(1) -mst(2) -mst(3);
          mst(1) 0 mst(3) -mst(2);
          mst(2) -mst(3) 0 mst(1);
          mst(3) mst(2) -mst(1) 0;];
    dst=sqrt(mst(1)^2+mst(2)^2+mst(3)^2);
    dst1=1-dst^2/8+dst^4/384;
    dst2=1/2-dst^2/48;
    mq=(dst1*eye(4)+dst2*mita)*mq;        
    mqq=sqrt(mq(1)^2+mq(2)^2+mq(3)^2+mq(4)^2);
    mq=mq/mqq;
    mCbn=[mq(1)^2+mq(2)^2-mq(3)^2-mq(4)^2,2*(mq(2)*mq(3)-mq(1)*mq(4)),2*(mq(2)*mq(4)+mq(1)*mq(3));
          2*(mq(2)*mq(3)+mq(1)*mq(4)),mq(1)^2-mq(2)^2+mq(3)^2-mq(4)^2,2*(mq(3)*mq(4)-mq(1)*mq(2));
          2*(mq(2)*mq(4)-mq(1)*mq(3)),2*(mq(3)*mq(4)+mq(1)*mq(2)),mq(1)^2-mq(2)^2-mq(3)^2+mq(4)^2;];
    T11=mCbn(1,1);T12=mCbn(1,2);T13=mCbn(1,3);
    T21=mCbn(2,1);T22=mCbn(2,2);T23=mCbn(2,3);
    T31=mCbn(3,1);T32=mCbn(3,2);T33=mCbn(3,3);
    mp=asin(mCbn(2,1));
    my=atan(-mCbn(3,1)/mCbn(1,1));  
           if(mCbn(1,1)<0)            
               if(my>0)                
                   my=my-pi;                
               else                
                   my=my+pi;
               end
           end
    mr=atan(-mCbn(2,3)/mCbn(2,2));
           if(mCbn(2,2)<0)            
               if(mr>0)                
                   mr=mr-pi;                
               else                
                   mr=mr+pi;
               end
           end
    mfi=[mr my mp];
   
    gn=[0 -g0 0];
    mdgc=gn*Tins; 
    mv1=mwains1(4:6)*g0*Tins/2;
    mv2=mwains2(4:6)*g0*Tins/2; 
    mvinsn=mCbn11*[mv1+mv2]';
    mvins=mvins+mvinsn'+mdgc;
    mpins=mpins+([mvins(1)/(Rn+mh) mvins(3)/(Re+mh)/cos(mL) mvins(2)])*Tins;
    mm(i,:)=[mfi*180/pi mvins mpins(1) mpins(2) mpins(3)];
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    m11=[mWiex;mWiey;mWiez;];
    m10=-[0 -m11(3) m11(2);m11(3) 0 -m11(1);-m11(2) m11(1) 0;];
    m20=[0 0 1/(Re+mh);0 0 tan(mL)/(Re+mh);-1/(Rn+mh) 0 0;];
    m30=[-Wie*sin(mL) 0 0;Wie*cos(mL) 0 0;0 0 0;];
    m40=-mCbn;
    mf=mCbn*[(mwains1(4:6)*g0+mwains2(4:6)*g0)/2]';
    m50=[0 0 mf(2);0 1 0;-mf(2) 0 0;];
    m60=[0 0 -2*Wie*sin(mL);0 0 2*Wie*cos(mL);2*Wie*sin(mL) -2*Wie*cos(mL) 0;];
    m70=mCbn;
    m80=[1/(Re+mh) 0 0;0 0 sec(mL)/(Re+mh);0 1 0;];
    m90=zeros(3,3);
    
    mFins3=[m10 m20 m30;
            m50 m60 m90;
            m90 m80 m90;];       
    mHk3=[m90 eye(3) m90;];
    mZk3=[mvins]';
    
    mFk3=eye(9)+Tins*mFins3 +Tins^2/2*mFins3^2 +Tins^3/6*mFins3^3 +Tins^4/24*mFins3^4 +Tins^5/120*mFins3^5; 
    m1=mQt3; 
    m2=mFins3*m1+(mFins3*m1)';
    m3=mFins3*m2+(mFins3*m2)'; 
    m4=mFins3*m3+(mFins3*m3)'; 
    m5=mFins3*m4+(mFins3*m4)';
    mQk3=m1*Tins +m2*Tins^2/2 +m3*Tins^3/6 +m4*Tins^4/24 +m5*Tins^5/120;

    mPkk3=mFk3*mPk3*mFk3'+mQk3;                       
    mKk3=mPkk3*mHk3'*(mHk3*mPkk3*mHk3'+mRk3)^-1;             
    mPk3=(eye(9)-mKk3*mHk3)*mPkk3;
    mXkk3=mFk3*mXk3;    
    mXk3=mXkk3+mKk3*(mZk3-mHk3*mXkk3); 
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
    fprintf(fid10,'%8.7f %8.7f %8.7f %8.7f %8.7f %8.7f %8.7f %8.7f %8.7f\n',mXk3'); 
    mxx3(k,:)=[mXk3'];
end
fclose(fid10);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%