利用MATLAB编制的GPS单点定位程序

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+1720981.5;t=JD-2444244.5;function head,obs=ReadObsData%读接收机观测数据文件%HeadODat :a structure stores header information if o-file% .ApproXYZ3; /approximate 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 Julian 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_FreL1SatSum; % .Obs_FreL2SatSum;% .Obs_RangeC1SatSum;% .Obs_RangeP1SatSum;% .Obs_RangeP2SatSum;global HeadODat;global ObsODat; fname,fpath=uigetfile(*.*,选择一个O文件); O_filename=strcat(fpath,fname); fid1=fopen(O_filename,rt); if (fid1=-1) msgbox(file invalide,warning,warn); return; end %将文件头数据存入结构体HeadODat中 t=0; while(t100) s=fgets(fid1); t=t+1; L=size(s,2); if L5 sg=fgets(fid1); s=strcat(s,sg); end L=size(s,2); %补充数据长度 if L1 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 HeadODat.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 %完成一个卫星的所有观测数据存储 end %完成一个历元的数据存储 end %完成所有历元的数据存储 head=HeadODat; obs=ObsODat; return function 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); 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),str2double(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); temp=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(temp(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(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) %删除重复数据 for k= i-1:i if (EphDat(i-1).PRN=EphDat(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;%多个历元加权平均求测站点坐标N = size(EphDat,2);c=299792458;epochNUM=size(ObsODat,2);%观测数据的个数Xk0=HeadODat.ApproXYZ(1,1); %测站点的近似坐标Yk0=HeadODat.ApproXYZ(1,2);Zk0=HeadODat.ApproXYZ(1,3);for k=1:epochNUM tr=ObsODat(k).TimeOEp; %历元接收数据时间 Snum=ObsODat(1,k).SatSum; %该历元观测到的卫星数 if Snum4 break; %去除无解的历元 end Code=ObsODat(k).SatCode; %该历元观测到的卫星号组 R=ObsODat(k).Obs_RangeC1 ; %取出C1观测值,列向量 %卫星发射时间的迭代计算 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 %卫星钟差 dd=-1; for i=1:N if(EphDat(i).PRN=Code(j)&abs(t-EphDat(i).toc)1E-8) tt(6) = ts; Xs1,Ys1,Zs1= CalPos(Code(j),tt); ss1 = sqrt(Xk0-Xs1)2+(Yk0-Ys1)2+(Zk0-Zs1)*(Zk0-Zs1); %卫星位置加地球自转改正 qe=0.000072921151467; %地球自转角速度 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; EAB=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; %构造误差方程 L = L;(R(j)-ss+c*dt)-2.47/(sin(EAB)+0.0121);%常数项 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/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; %构造法方程并求解 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); endendXp=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=TimetoJD(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