guiji0.m

惯性导航仿真matlab程序

% %轨迹设计程序
clc;
clear;

%参量初始化
Rr=6378160;e=1/298.3;Wie=7.2921151467e-5;g0=9.7803267714;

%龙格-库塔步长设计，数据更新周期为0.002s
T=0.010;

%初始姿态角为零度
p=0*pi/180;y=0*pi/180;r=0*pi/180;

%初始转换矩阵
Cbn=[cos(p)*cos(y),-sin(p)*cos(y)*cos(r)+sin(y)*sin(r),sin(p)*cos(y)*sin(r)+sin(y)*cos(r);
     sin(p),cos(p)*cos(r),-cos(p)*sin(r);
    -sin(y)*cos(p),sin(y)*sin(p)*cos(r)+cos(y)*sin(r),-sin(p)*sin(y)*sin(r)+cos(y)*cos(r);]; 

%初始速度为0m/s
ve=0;vn=0;vu=0;

%初始经度0 纬度0 高度距地100

L=0;
J=0;
h=0;

%初始信息[滚转角 偏航角 俯仰角 北向速度 天向速度 东向速度 纬度 经度 高度 陀螺仪输出信息 加速度计输出信息]
xinxi=[r y p vn vu ve L J h 0 0 0 0 g0 0]'; 

fid = fopen('cout.txt','w+'); 

%gwa()轨迹信息提取
wa=gwa(0);

%龙格库塔解算
for k=1:20000        
    k1=xinxijs(xinxi,wa);
    wa=(wa+gwa(k))/2; 
    k2=xinxijs(xinxi+T/2*k1,wa);
    k3=xinxijs(xinxi+T/2*k2,wa);
    wa=gwa(k); 
    k4=xinxijs(xinxi+T*k3,wa);
    xinxi=xinxi+T/6*(k1+2*k2+2*k3+k4);
    rr=[xinxi(1:9)',(k1(10:15)'+k2(10:15)'+k3(10:15)'+k4(10:15)')/4];
    fprintf(fid,'%6.5f %6.5f %6.5f %6.5f %6.5f %6.5f %6.5f %6.5f %6.5f %6.5f %6.5f %6.5f %6.5f %6.5f %6.5f\n',rr); 
    rr0(k,:)=wa';
end
fclose(fid);

load cout.txt;
%初始对准
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
mp=0;
mr=0;
my=0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
mvn=0;mvu=0;mve=0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%地球自转角速率、地球半径、曲率半径、经度、纬度、高度、重力加速度
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Wie=0.000072722;
Rr=6378245;
e=1/298.3;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
L=0;
h=0;
J=0;
Tins=0.020;
g00=9.7803267714;
g0=g00*(1+5.27094e-3*sin(L)^2+2.32718e-5*sin(L)^4)-3.086e-6*h;

x00=[mr my mp];
q=[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=[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];
  
mCbn11=[cos(mp)*cos(my),-sin(mp)*cos(my)*cos(mr)+sin(my)*sin(mr),sin(mp)*cos(my)*sin(mr)+sin(my)*cos(mr);
       sin(mp),cos(mp)*cos(mr),-cos(mp)*sin(mr);
       -sin(my)*cos(mp),sin(my)*sin(mp)*cos(mr)+cos(my)*sin(mr),-sin(mp)*sin(my)*sin(mr)+cos(my)*cos(mr);];

mvins=[0 0 0]; 
mpins=[L J h];
mvins1=mvins;
mpins1=mpins;  
dd=length(cout)/2;
for i=1:10000
    mwains1=cout(2*i-1,10:15);
    mwains2=cout(2*i,10:15);
    g0=g00*(1+5.27094e-3*sin(mpins(1))^2+2.32718e-5*sin(mpins(1))^4)-3.086e-6*mpins(3);
    Rn=(1-2*e+3*e*sin(mpins(1))^2)*Rr;
    Re=(1+e*sin(mpins(1))^2)*Rr;

    mCbn0=mCbn;
    mpins0=mpins;
    mvins0=mvins;
    mvins=3/2*mvins-1/2*mvins1;
    mpins=3/2*mpins-1/2*mpins1;
    Wiex=Wie*cos(mpins(1));
    Wiey=Wie*sin(mpins(1));
    Wiez=0;
    Wenx=mvins(3)/(Re+mpins(3));
    Weny=mvins(3)*tan(mpins(1))/(Re+mpins(3));
    Wenz=-mvins(1)/(Rn+mpins(3));
    Win=mCbn'*[Wiex+Wenx;Wiey+Weny;Wiez+Wenz];
    Wnb1=mwains1(1:3)-Win';
    Wnb2=mwains2(1:3)-Win';
    webb(2*i-1,1:3)=Wnb1;
    webb(2*i,1:3)=Wnb2;
  
    st1=Wnb1*Tins/2;
    st2=Wnb2*Tins/2;
    sst=st1+st2+2/3*cross(st1,st2);
    sita=[0 -sst(1) -sst(2) -sst(3);
          sst(1) 0 sst(3) -sst(2);
          sst(2) -sst(3) 0 sst(1);
          sst(3) sst(2) -sst(1) 0;];
   
    dst=sqrt(sst(1)^2+sst(2)^2+sst(3)^2);
    dst1=1-dst^2/8+dst^4/384;
    dst2=1/2-dst^2/48;
    q=(dst1*eye(4)+dst2*sita)*q;        
    qq=sqrt(q(1)^2+q(2)^2+q(3)^2+q(4)^2);
    q=q/qq;
         
    Cbn=[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];
    
    T11=Cbn(1,1);T12=Cbn(1,2);T13=Cbn(1,3);
    T21=Cbn(2,1);T22=Cbn(2,2);T23=Cbn(2,3);
    T31=Cbn(3,1);T32=Cbn(3,2);T33=Cbn(3,3);
    p=asin(Cbn(2,1));
    y=atan(-Cbn(3,1)/Cbn(1,1));  
           if(Cbn(1,1)<0)            
               if(y>0)                
                   y=y-pi;                
               else                
                   y=y+pi;
               end
           end
    r=atan(-Cbn(2,3)/Cbn(2,2));
           if(Cbn(2,2)<0)            
               if(r>0)                
                   r=r-pi;                
               else                
                   r=r+pi;
               end
           end
    fi=[r y p];
   
    mCbn=Cbn;  
    
    gn=[0 -g0 0];
    sdgc=(gn-cross([2*Wiex+Wenx 2*Wiey+Weny 2*Wiez+Wenz],mvins))*Tins; 
    sv1=mwains1(4:6)*Tins/2;
    sv2=mwains2(4:6)*Tins/2;
    
    cnn=[1 0 0;0 1 0;0 0 1;]-[0 -(Wiez+Wenz) (Wiey+Weny);(Wiez+Wenz) 0 -(Wiex+Wenx);-(Wiey+Weny) (Wiex+Wenx) 0;];
%     mvinsn=mCbn*[sv1+sv2+1/2*cross(st1+st2,sv1+sv2)+2/3*(cross(st1,sv2)+cross(sv1,st2))]';
%     mvinsn=-0.5*(-cnn+eye(3))*mCbn0*[sv1+sv2]'+mCbn0*([[sv1+sv2]+1/2*cross(st1+st2,sv1+sv2)+2/3*(cross(st1,sv2)+cross(sv1,st2))]');
    mvinsn=cnn*mCbn0*([sv1+sv2]'+[1/2*cross(st1+st2,sv1+sv2)+2/3*(cross(st1,sv2)+cross(sv1,st2))]');
    dmvins=mvinsn'+sdgc;
    mvins=mvins+mvinsn'+sdgc;
    
    mvins1=mvins0;
    mpins=mpins+([(mvins(1)+mvins0(1))/2/(Rn+mpins(3)) (mvins(3)+mvins0(3))/2/((Re+mpins(3))/cos(mpins(1))) (mvins(2)+mvins0(2)/2)])*Tins;
    mpins1=mpins0;
%     mm(i,:)==[i*0.020 (fi-x00)*180/pi mvins (mpins(1)-0.594282)*Rn (mpins(2)-1.901103)*Re mpins(3)-489];
    jiesuanxinxi(i,:)=[fi mvins mpins];
    i
%     fprintf(fid1,'%6.3f %8.7f %8.7f %8.7f %8.7f %8.7f %8.7f %8.7f %8.7f %8.7f\n',mm);
end