利用MATLAB編制的GPS單點(diǎn)定位程序

1、精選優(yōu)質(zhì)文檔-傾情為你奉上function t=TimetoJD(Y,M,D,h,f,s)if(Y>=80) Y=Y+1900;else Y=Y+2000;endif M<=2 Y=Y-1; M=M+12;endJD=fix(365.25*Y)+fix(30.6001*(M+1)+D+h/24+f/1440+s/24/3600+.5;t=JD-.5;function head,obs=ReadObsData%讀接收機(jī)觀測(cè)數(shù)據(jù)文件%HeadODat :a structure stores header information if o-file% .ApproXYZ3; /appro

2、ximate coordinate% .interval; /intervals of two adjacent epochs% .SiteName; /point name% .Ant_H; /antenna height% .Ant_E; /east offset of the antenna center% .Ant_N; /north offset of then antenna center% .TimeOB; /first epoch time in modifuied Julian time% .TimeOE; /last epoch time in modifuied Juli

3、an time% .SumOType; /number of types of observables% .SumOOSumOType; /type of observables 0-L1,1-L2,2-C1,3-P1,4-P2,5-D1,6-D2,%ObsODat :a structure stores observables by one and one epoch% .TimeOEpp2; /reciever time of epoch% .SatSum; /number of satellites% .SatCodeSatSum; /satellites' PRN% .Obs_

4、FreL1SatSum; % .Obs_FreL2SatSum;% .Obs_RangeC1SatSum;% .Obs_RangeP1SatSum;% .Obs_RangeP2SatSum;global HeadODat;global ObsODat; fname,fpath=uigetfile('*.*','選擇一個(gè)O文件'); O_filename=strcat(fpath,fname); fid1=fopen(O_filename,'rt'); if (fid1=-1) msgbox('file invalide','

5、;warning','warn'); return; end %將文件頭數(shù)據(jù)存入結(jié)構(gòu)體HeadODat中 t=0; while(t<100) s=fgets(fid1); t=t+1; L=size(s,2); if L<81 s(L+1:81)=' ' end instrS=s(61:81); %測(cè)站點(diǎn)近似坐標(biāo) if strncmp(instrS,'APPROX POSITION XYZ',19) HeadODat.ApproXYZ=zeros(1,3); HeadODat.ApproXYZ(1,1)=str2num(s(1

6、:14); HeadODat.ApproXYZ(1,2)=str2num(s(15:28); HeadODat.ApproXYZ(1,3)=str2num(s(29:42); %歷元間隔; elseif strncmp(instrS,'INTERVAL',8) HeadODerval=str2num(s(5:11); %測(cè)站點(diǎn)號(hào) elseif strncmp(instrS,'MARKER NAME',11) HeadODat.SiteName=s(1:4) %天線中心改正 elseif strncmp(instrS,'ANTENNA: DEL

7、TA H/E/N',20) HeadODat.Ant_H=str2num(s(1:14); HeadODat.Ant_E=str2num(s(15:28); HeadODat.Ant_N=str2num(s(29:42); %第一個(gè)歷元時(shí)間 elseif strncmp(instrS,'TIME OF FIRST OBS',17) year=str2num(s(1:6); month=str2num(s(7:12); day=str2num(s(13:18); hour=str2num(s(19:24); minute=str2num(s(25:30); second=

8、str2num(s(31:42); HeadODat.TimeOB=TimetoJD(year,month,day,hour,minute,second); %最后一個(gè)歷元時(shí)間 elseif strncmp(instrS,'TIME OF LAST OBS',16) year=str2num(s(1:6); month=str2num(s(7:12); day=str2num(s(13:18); hour=str2num(s(19:24); minute=str2num(s(25:30); second=str2num(s(31:42); HeadODat.TimeOE=Tim

9、etoJD(year,month,day,hour,minute,second); %觀測(cè)值類(lèi)型 elseif strncmp(instrS,'# / TYPES OF OBSERV',19) HeadODat.SumOType=str2num(s(1:6); HeadODat.SumOO=ones(1,HeadODat.SumOType)*-1; for k=1:HeadODat.SumOType f=s(k*6+5:k*6+6); if strcmp(f,'L1') HeadODat.SumOO(1,k)=0; elseif strcmp(f,'L2

10、') HeadODat.SumOO(1,k)=1; elseif strcmp(f,'C1') HeadODat.SumOO(1,k)=2; elseif strcmp(f,'P1') HeadODat.SumOO(1,k)=3; elseif strcmp(f,'P2') HeadODat.SumOO(1,k)=4; elseif strcmp(f,'D1') HeadODat.SumOO(1,k)=5; elseif strcmp(f,'D2') HeadODat.SumOO(1,k)=6; end e

11、nd %頭文件結(jié)束 elseif strncmp(instrS,'END OF HEADER',13) break; else continue; end end %觀測(cè)數(shù)據(jù)結(jié)構(gòu)體%觀測(cè)數(shù)據(jù)結(jié)構(gòu) t=0; while feof(fid1) %每個(gè)歷元的第一行數(shù)據(jù)，時(shí)間和觀測(cè)到的衛(wèi)星號(hào) s=fgets(fid1); t=t+1; year=str2num(s(1:3); month=str2num(s(4:6); day=str2num(s(7:9); hour=str2num(s(10:12); minute=str2num(s(13:15); second=str2num(s

12、(16:26); %歷元時(shí)間 ObsODat(t).TimeOEp=year,month,day,hour,minute,second; ObsODat(t).TimeOEpp=TimetoJD(year,month,day,hour,minute,second); %該歷元觀測(cè)到的衛(wèi)星數(shù) ObsODat(t).SatSum=str2num(s(30:32); %該歷元觀測(cè)到的衛(wèi)星號(hào) ObsODat(t).SatCode=zeros(1,ObsODat(t).SatSum); ObsODat(t).Obs_FreL1=zeros(1,ObsODat(t).SatSum); ObsODat(t)

13、.Obs_FreL2=zeros(1,ObsODat(t).SatSum); ObsODat(t).Obs_RangeC1=zeros(1,ObsODat(t).SatSum); ObsODat(t).Obs_RangeP1=zeros(1,ObsODat(t).SatSum); ObsODat(t).Obs_RangeP2=zeros(1,ObsODat(t).SatSum); for k=1:ObsODat(t).SatSum f=s(31+k*3:32+k*3); ObsODat(t).SatCode(1,k)=str2num(f); end %每個(gè)歷元的觀測(cè)數(shù)據(jù)，按衛(wèi)星號(hào)先后順序分行存

14、 for k=1:ObsODat(t).SatSum s=fgets(fid1); %判斷一個(gè)衛(wèi)星的觀測(cè)數(shù)據(jù)是否分兩行存儲(chǔ)，如果為兩行，則再讀入一行 if HeadODat.SumOType>5 sg=fgets(fid1); s=strcat(s,sg); end L=size(s,2); %補(bǔ)充數(shù)據(jù)長(zhǎng)度 if L<HeadODat.SumOType*16 s(L+1:HeadODat.SumOType*16)=' ' end %對(duì)觀測(cè)數(shù)據(jù)判斷其類(lèi)型，并存儲(chǔ)到相應(yīng)的數(shù)組中 for j=1:HeadODat.SumOType stemp=s(j*16-15:j*16

15、); stemp=deblank(stemp); if isempty(stemp) continue; end stempNum=str2num(stemp); stempLength=size(stempNum,2); if stempLength>1 stempNum=stempNum(1,1); end if HeadODat.SumOO(1,j)=0 ObsODat(t).Obs_FreL1(1,k)=stempNum; elseif HeadODat.SumOO(1,j)=1 ObsODat(t).Obs_FreL2(1,k)=stempNum; elseif HeadODa

16、t.SumOO(1,j)=2 ObsODat(t).Obs_RangeC1(1,k)=stempNum; elseif HeadODat.SumOO(1,j)=3 ObsODat(t).Obs_RangeP1(1,k)=stempNum; elseif HeadODat.SumOO(1,j)=4 ObsODat(t).Obs_RangeP2(1,k)=stempNum; else continue; end end %完成一個(gè)衛(wèi)星的所有觀測(cè)數(shù)據(jù)存儲(chǔ) end %完成一個(gè)歷元的數(shù)據(jù)存儲(chǔ) end %完成所有歷元的數(shù)據(jù)存儲(chǔ) head=HeadODat; obs=ObsODat; return func

17、tion EphDat=ReadGpsDataglobal EphDatEphDat=struct;filename,pathname=uigetfile('*.*N','讀取GPS廣播星歷文件');fid1=fopen(strcat(pathname,filename),'rt');if(fid1=-1) msgbox('Input File or Path is not correct','warning ','warn'); return;endwhile(1) temp=fgetl(fid1

18、); header=findstr(temp,'END OF HEADER'); if(isempty(header) break; endendi=1;while(1) temp=fgetl(fid1); if(temp=-1) break; end EphDat(i).PRN=str2double(temp(1:2); year=str2double(temp(3:5); EphDat(i).toc=TimetoJD(year,str2double(temp(6:8),str2double(temp(9:11),str2double(temp(12:14),str2doub

19、le(temp(15:17),str2double(temp(18:22); EphDat(i).a0=str2num(temp(23:41); EphDat(i).a1=str2num(temp(42:60); EphDat(i).a2=str2num(temp(61:79); temp=fgetl(fid1); EphDat(i).idoe=str2num(temp(4:22); EphDat(i).Crs=str2num(temp(23:41); EphDat(i).dn=str2num(temp(42:60); EphDat(i).M0=str2num(temp(61:79); tem

20、p=fgetl(fid1); EphDat(i).Cuc=str2num(temp(4:22); EphDat(i).e=str2num(temp(23:41); EphDat(i).Cus=str2num(temp(42:60); EphDat(i).sqa=str2num(temp(61:79); temp=fgetl(fid1); EphDat(i).toe=str2num(temp(4:22); EphDat(i).Cic=str2num(temp(23:41); EphDat(i).omg0=str2num(temp(42:60); EphDat(i).Cis=str2num(tem

21、p(61:79); temp=fgetl(fid1); EphDat(i).i0=str2num(temp(4:22); EphDat(i).Crc=str2num(temp(23:41); EphDat(i).w=str2num(temp(42:60); EphDat(i).omg=str2num(temp(61:79); temp=fgetl(fid1); EphDat(i).iDot=str2num(temp(4:22); EphDat(i).cflgl2=str2num(temp(23:41); EphDat(i).weekno=str2num(temp(42:60); EphDat(

22、i).pflgl2=str2num(temp(61:79); temp=fgetl(fid1); EphDat(i).svacc=str2num(temp(4:22); EphDat(i).svhlth=str2num(temp(23:41); EphDat(i).tgd=str2num(temp(42:60); EphDat(i).aodc=str2num(temp(61:79); temp=fgetl(fid1); EphDat(i).ttm=str2num(temp(4:22); if(i=1) %刪除重復(fù)數(shù)據(jù) for k= i-1:i if (EphDat(i-1).PRN=EphDa

23、t(i).PRN) break; elseif abs(EphDat(i-1).toc-EphDat(i).toc)<0.001 i=i - 1; end end end i=i + 1;endfunction DDDWformat longclear alltic global HeadODat; global ObsODat; global EphDat;%先讀N文件，再讀O文件EphDat=ReadGpsData;HeadODat,ObsODat=ReadObsData;%多個(gè)歷元加權(quán)平均求測(cè)站點(diǎn)坐標(biāo)N = size(EphDat,2);c=;epochNUM=size(ObsOD

24、at,2);%觀測(cè)數(shù)據(jù)的個(gè)數(shù)Xk0=HeadODat.ApproXYZ(1,1); %測(cè)站點(diǎn)的近似坐標(biāo)Yk0=HeadODat.ApproXYZ(1,2);Zk0=HeadODat.ApproXYZ(1,3);for k=1:epochNUM tr=ObsODat(k).TimeOEp; %歷元接收數(shù)據(jù)時(shí)間 Snum=ObsODat(1,k).SatSum; %該歷元觀測(cè)到的衛(wèi)星數(shù) if Snum<4 break; %去除無(wú)解的歷元 end Code=ObsODat(k).SatCode; %該歷元觀測(cè)到的衛(wèi)星號(hào)組 R=ObsODat(k).Obs_RangeC1 ; %取出C1觀測(cè)值,

25、列向量 %衛(wèi)星發(fā)射時(shí)間的迭代計(jì)算 for j=1:Snum if R(j) = 0 continue; end t=TimetoJD(tr(1),tr(2),tr(3),tr(4),tr(5),tr(6); t2 = mod(t,7)*24*3600;%gps second %衛(wèi)星鐘差 dd=-1; for i=1:N if(EphDat(i).PRN=Code(j)&abs(t-EphDat(i).toc)<0.0417*2)%讀入時(shí)間相近的星歷文件 a0= EphDat(i).a0; a1=EphDat(i).a1; a2= EphDat(i).a2; toe=EphDat(

26、i).toe; dt=a0+(t2-toe)*a1+(t2-toe)2*a2;%衛(wèi)星鐘差 dd=1; break; end end if dd=-1 msgbox('沒(méi)有相關(guān)星歷文件'); return; end tt = tr; ts = tr(6) - R(j)/c;%用秒進(jìn)行迭代 sign = 1; while(sign>1E-8) tt(6) = ts; Xs1,Ys1,Zs1= CalPos(Code(j),tt); ss1 = sqrt(Xk0-Xs1)2+(Yk0-Ys1)2+(Zk0-Zs1)*(Zk0-Zs1); %衛(wèi)星位置加地球自轉(zhuǎn)改正 qe=0.14

27、67; %地球自轉(zhuǎn)角速度 delt=ss1/c; Rotation=cos(qe*delt),sin(qe*delt),0;-sin(qe*delt),cos(qe*delt),0;0,0,1; h=Rotation*Xs1;Ys1;Zs1 ; Xs=h(1); Ys=h(2); Zs=h(3); ss = sqrt(Xk0-Xs)2+(Yk0-Ys)2+(Zk0-Zs)*(Zk0-Zs); ts = tr(6)-ss/c; sign = norm(ts-tt(6); end axk = (Xk0-Xs)/ss; ayk = (Yk0-Ys)/ss; azk = (Zk0-Zs)/ss; EA

28、B=CAL2POL(Xk0,Yk0,Zk0,Xs,Ys,Zs); if j=1 A = axk,ayk,azk,1; L = R(j)-ss+c*dt-2.47/(sin(EAB)+0.0121); else A = A;axk,ayk,azk,1; %構(gòu)造誤差方程 L = L;(R(j)-ss+c*dt)-2.47/(sin(EAB)+0.0121);%常數(shù)項(xiàng) end end if Snum=4 dx=inv(A)*L; aaaa(k).Q=inv(A'*A); Px(k) = 1/aaaa(k).Q(1,1); Py(k) = 1/aaaa(k).Q(2,2); Pz(k) = 1

29、/aaaa(k).Q(3,3); xchange(k) = dx(1); ychange(k) = dx(2); zchange(k) = dx(3); elseif Snum > 4 dx = inv(A'*A)*A'*L; %構(gòu)造法方程并求解 aaaa(k).Q=inv(A'*A); Px(k) = 1/aaaa(k).Q(1,1); Py(k) = 1/aaaa(k).Q(2,2); Pz(k) = 1/aaaa(k).Q(3,3); xchange(k) = dx(1); ychange(k) = dx(2); zchange(k) = dx(3); en

30、dendXp=sum(Px.*(Xk0+xchange)/sum(Px)Yp=sum(Py.*(Yk0+ychange)/sum(Py)Zp=sum(Pz.*(Zk0+zchange)/sum(Pz)figure(1);subplot(3,1,1);plot(xchange,'black-');subplot(3,1,2);plot(ychange,'black-');subplot(3,1,3);plot(zchange,'black-');tocfunction x,y,z= CalPos(PRN,time)global EphDatt1=T

31、imetoJD(time(1),time(2),time(3),time(4),time(5),time(6);t2=TimetoJD(time(1),time(2),time(3),0,0,0);if isempty(EphDat) EphDat=ReadGpsData;endsz=size(EphDat,2);gg=0;%判斷最近的時(shí)間for i=1:sz if(EphDat(i).PRN=PRN & abs(EphDat(i).toc-t1)<0.0417*2) G=EphDat(i); gg=1; break; endendt3=t2-G.weekno*7;% 減整周 數(shù)

32、t=t3*24*3600+time(4)*3600+time(5)*60+time(6);%化為GPS秒u=3.E+14; %地球引力常數(shù)we=7.E-5; %地球自轉(zhuǎn)速度a=G.sqa2; %地球長(zhǎng)半軸n0=sqrt(u/a3); %計(jì)算的平運(yùn)動(dòng) tk=t-G.toe; %從參考?xì)v元開(kāi)始的時(shí)間 if tk>= tk=tk-;elseif tk<- tk=tk + ;endn=n0+G.dn; %改正后的運(yùn)動(dòng) Mk=G.M0+n*tk;Ek=Mk; %偏近點(diǎn)角 弧度f(wàn)or i=1:4 Ek=Mk+G.e*sin(Ek);endfk=2*atan(sqrt(1+G.e)/(1-G.e

33、)*tan(Ek/2); %真近點(diǎn)角if(fk<0) fk=fk+2*pi;endcosf=(cos(Ek)-G.e)/(1-G.e*cos(Ek);sinf=(sqrt(1-G.e2)*sin(Ek)/(1-G.e*cos(Ek);uk=fk+G.w; %升交角矩及軌道攝動(dòng)改正項(xiàng)iuk=G.Cuc*cos(2*uk)+G.Cus*sin(2*uk);irk=G.Crc*cos(2*uk)+G.Crs*sin(2*uk);iik=G.Cic*cos(2*uk)+G.Cis*sin(2*uk);uk=uk+iuk ; %改正后的升角距r=a*(1-G.e*cos(Ek)+irk; %改正后的地心相徑i=G.i0+iik+G.iDot*tk; %改正后的傾角 xx=r*cos(uk); %在軌道面中的位置yy=r*sin(uk);omg=G.omg0+(G.omg-we)*tk-we*G.toe; %改正后的升交點(diǎn)經(jīng)度x=xx*cos(omg)-yy*cos(i)*sin(omg);y=xx*sin(omg)+yy*cos(i)*cos(omg);z=yy*sin(i);CalPos=x,y,z;%打印結(jié)果% x