dhjs.m

一个惯性导航解算程序

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 惯性导航系统
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc;
clear all;
close all;

load gps4.dat;
gps1=gps4;
load swains4.txt;
swains=swains4;
my0=(69.020000)/(180/pi);
my=my0;
mL=0.00;mh=00;mJ=00;  

format long
mr=-0.00000/57.3;
mp=0/57.3;
Wie=0.000072722;Rr=6378245;e=1/298.3;g0=9.7803267714;g00=9.7803267714;
Tins=0.010;
mvn=0;mvu=0;mve=0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
g0=g00*(1+5.27094e-3*sin(mL)^2+2.32718e-5*sin(mL)^4)-3.086e-6*mh;
mma=mean(swains(100:3000,4:6));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x00=[mr my mp]
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];  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
mwbias0=mean(swains(1000:3000,1:3))-[mCbn'*[Wie*cos(mL) Wie*sin(mL) 0]']'
mabias0=mean(swains(1000:3000,4:6))-[mCbn'*[0 1 0]']'
% mwbias0=mean(mxx3(2000:3500,10:12))
% mabias0=mean(mxx3(2000:3500,13:15))
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
mvins2=[0 0 0]; mpins2=[mL mJ mh];
mvins1=[0 0 0]; mpins1=[mL mJ mh];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fid3 = fopen('mgdjs.txt','w+');
fid8 = fopen('gpsvp.txt','w+');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for k=1:23600000
    i=k;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %主惯导导航解算
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    swains1=swains(2*k-1+20+0,1:6)-[mwbias0 mabias0];
    swains2=swains(2*k+20+0,1:6)-[mwbias0 mabias0];
    mvins0=3/2*mvins2-1/2*mvins1;
    mpins0=3/2*mpins2-1/2*mpins1;
    g0=g00*(1+5.27094e-3*sin(mpins0(1))^2+2.32718e-5*sin(mpins0(1))^4)-3.086e-6*mpins0(3);
    Rn=(1-2*e+3*e*sin(mpins0(1))^2)*Rr;
    Re=(1+e*sin(mpins0(1))^2)*Rr;
    mCbn0=mCbn;
    mWiex=Wie*cos(mpins0(1));
    mWiey=Wie*sin(mpins0(1));
    mWiez=0;
    mWenx=mvins0(3)/(Re+mpins0(3));
    mWeny=mvins0(3)*tan(mpins0(1))/(Re+mpins0(3));
    mWenz=-mvins0(1)/(Rn+mpins0(3));
    mWin=mCbn'*[mWiex+mWenx;mWiey+mWeny;mWiez+mWenz];
    mWnb1=swains1(1:3)-mWin';
    mWnb2=swains2(1:3)-mWin';
    webb(2*k-1,1:3)=mWnb1;
    webb(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                
                   r=r+pi;
               end
           end
    mfi=[mr my mp];   
    mCbn=mCbn;    
    gn=[0 -g0 0];
    mdgc=(gn-cross([2*mWiex+mWenx 2*mWiey+mWeny 2*mWiez+mWenz],mvins0))*Tins; 
    mv1=swains1(4:6)*g0*Tins/2;
    mv2=swains2(4:6)*g0*Tins/2;
    cnn=[1 0 0;0 1 0;0 0 1;]-[0 -(mWiez+mWenz) (mWiey+mWeny);(mWiez+mWenz) 0 -(mWiex+mWenx);-(mWiey+mWeny) (mWiex+mWenx) 0;];
    mvinsn=-0.5*(-cnn+eye(3))*mCbn0*[mv1+mv2]'+mCbn0*([[mv1+mv2]+1/2*cross(mt1+mt2,mv1+mv2)+2/3*(cross(mt1,mv2)+cross(mv1,mt2))]');
    mvins1=mvins2;
    mvins=mvins2+mvinsn'+mdgc; 
    mvins2=mvins; 
    mpins1=mpins2;
    mpins=mpins2+([(mvins1(1)+mvins2(1))/2/(Rn+mpins2(3)) (mvins1(3)+mvins2(3))/2/(Re+mpins2(3))/cos(mpins0(1)) (mvins1(2)+mvins2(2))/2])*Tins;
    mpins2=mpins;
    mgdjs0(k,:)=[mfi(1)*180/pi mfi(2)*180/pi-my0*57.3 mfi(3)*180/pi mvins (mpins(1)-mL)*Rn (mpins(2)-mJ)*Re mpins(3)-mh];
    fprintf(fid3,'%8.7f %8.7f %8.7f %8.7f %8.7f %8.7f %8.7f %8.7f %8.7f\n',mgdjs0(k,:)); 
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if mod(k,100)==0  
    k/100+40
    gpsvp0=[gps1(k/100,4:6) (gps1(k/100,1)-mL)*Rn (gps1(k/100,2)-mJ)*Re gps1(k/100,3)-mh];
    fprintf(fid8,'%8.7f %8.7f %8.7f %8.7f %8.7f %8.7f\n',gpsvp0); 
end     
end
fclose(fid3);
fclose(fid8);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%