利用MATLAB编制的GPS单点定位程序.docx
- 文档编号:4550271
- 上传时间:2022-12-06
- 格式:DOCX
- 页数:16
- 大小:19.18KB
利用MATLAB编制的GPS单点定位程序.docx
《利用MATLAB编制的GPS单点定位程序.docx》由会员分享,可在线阅读,更多相关《利用MATLAB编制的GPS单点定位程序.docx(16页珍藏版)》请在冰豆网上搜索。
利用MATLAB编制的GPS单点定位程序
functiont=TimetoJD(Y,M,D,h,f,s)
if(Y>=80)
Y=Y+1900;
else
Y=Y+2000;
end
ifM<=2
Y=Y-1;
M=M+12;
end
JD=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:
astructurestoresheaderinformationifo-file
%.ApproXYZ[3];//approximatecoordinate
%.interval;//intervalsoftwoadjacentepochs
%.SiteName;//pointname
%.Ant_H;//antennaheight
%.Ant_E;//eastoffsetoftheantennacenter
%.Ant_N;//northoffsetofthenantennacenter
%.TimeOB;//firstepochtimeinmodifuiedJuliantime
%.TimeOE;//lastepochtimeinmodifuiedJuliantime
%.SumOType;//numberoftypesofobservables
%.SumOO[SumOType];//typeofobservables0-L1,1-L2,2-C1,3-P1,4-P2,5-D1,6-D2,
%ObsODat:
astructurestoresobservablesbyoneandoneepoch
%.TimeOEpp[2];//recievertimeofepoch
%.SatSum;//numberofsatellites
%.SatCode[SatSum];//satellites'PRN
%.Obs_FreL1[SatSum];
%.Obs_FreL2[SatSum];
%.Obs_RangeC1[SatSum];
%.Obs_RangeP1[SatSum];
%.Obs_RangeP2[SatSum];
globalHeadODat;
globalObsODat;
[fname,fpath]=uigetfile('*.*','选择一个O文件');
O_filename=strcat(fpath,fname);
fid1=fopen(O_filename,'rt');
if(fid1==-1)
msgbox('fileinvalide','warning','warn');
return;
end
%将文件头数据存入结构体HeadODat中
t=0;
while(t<100)
s=fgets(fid1);
t=t+1;
L=size(s,2);
ifL<81
s(L+1:
81)='';
end
instrS=s(61:
81);
%测站点近似坐标
ifstrncmp(instrS,'APPROXPOSITIONXYZ',19)
HeadODat.ApproXYZ=zeros(1,3);
HeadODat.ApproXYZ(1,1)=str2num(s(1:
14));
HeadODat.ApproXYZ(1,2)=str2num(s(15:
28));
HeadODat.ApproXYZ(1,3)=str2num(s(29:
42));
%历元间隔;
elseifstrncmp(instrS,'INTERVAL',8)
HeadODat.interval=str2num(s(5:
11));
%测站点号
elseifstrncmp(instrS,'MARKERNAME',11)
HeadODat.SiteName=s(1:
4)
%天线中心改正
elseifstrncmp(instrS,'ANTENNA:
DELTAH/E/N',20)
HeadODat.Ant_H=str2num(s(1:
14));
HeadODat.Ant_E=str2num(s(15:
28));
HeadODat.Ant_N=str2num(s(29:
42));
%第一个历元时间
elseifstrncmp(instrS,'TIMEOFFIRSTOBS',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=str2num(s(31:
42));
HeadODat.TimeOB=TimetoJD(year,month,day,hour,minute,second);
%最后一个历元时间
elseifstrncmp(instrS,'TIMEOFLASTOBS',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=TimetoJD(year,month,day,hour,minute,second);
%观测值类型
elseifstrncmp(instrS,'#/TYPESOFOBSERV',19)
HeadODat.SumOType=str2num(s(1:
6));
HeadODat.SumOO=ones(1,HeadODat.SumOType)*-1;
fork=1:
HeadODat.SumOType
f=s(k*6+5:
k*6+6);
ifstrcmp(f,'L1')
HeadODat.SumOO(1,k)=0;
elseifstrcmp(f,'L2')
HeadODat.SumOO(1,k)=1;
elseifstrcmp(f,'C1')
HeadODat.SumOO(1,k)=2;
elseifstrcmp(f,'P1')
HeadODat.SumOO(1,k)=3;
elseifstrcmp(f,'P2')
HeadODat.SumOO(1,k)=4;
elseifstrcmp(f,'D1')
HeadODat.SumOO(1,k)=5;
elseifstrcmp(f,'D2')
HeadODat.SumOO(1,k)=6;
end
end
%头文件结束
elseifstrncmp(instrS,'ENDOFHEADER',13)
break;
else
continue;
end
end
%观测数据结构体%观测数据结构
t=0;
while~feof(fid1)
%每个历元的第一行数据,时间和观测到的卫星号
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(16:
26));
%历元时间
ObsODat(t).TimeOEp=[year,month,day,hour,minute,second];
ObsODat(t).TimeOEpp=TimetoJD(year,month,day,hour,minute,second);
%该历元观测到的卫星数
ObsODat(t).SatSum=str2num(s(30:
32));
%该历元观测到的卫星号
ObsODat(t).SatCode=zeros(1,ObsODat(t).SatSum);
ObsODat(t).Obs_FreL1=zeros(1,ObsODat(t).SatSum);
ObsODat(t).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);
fork=1:
ObsODat(t).SatSum
f=s(31+k*3:
32+k*3);
ObsODat(t).SatCode(1,k)=str2num(f);
end
%每个历元的观测数据,按卫星号先后顺序分行存
fork=1:
ObsODat(t).SatSum
s=fgets(fid1);
%判断一个卫星的观测数据是否分两行存储,如果为两行,则再读入一行
ifHeadODat.SumOType>5
sg=fgets(fid1);
s=strcat(s,sg);
end
L=size(s,2);
%补充数据长度
ifL s(L+1: HeadODat.SumOType*16)=''; end %对观测数据判断其类型,并存储到相应的数组中 forj=1: HeadODat.SumOType stemp=s(j*16-15: j*16); stemp=deblank(stemp); ifisempty(stemp) continue; end stempNum=str2num(stemp); stempLength=size(stempNum,2); ifstempLength>1 stempNum=stempNum(1,1); end ifHeadODat.SumOO(1,j)==0 ObsODat(t).Obs_FreL1(1,k)=stempNum; elseifHeadODat.SumOO(1,j)==1 ObsODat(t).Obs_FreL2(1,k)=stempNum; elseifHeadODat.SumOO(1,j)==2 ObsODat(t).Obs_RangeC1(1,k)=stempNum; elseifHeadODat.SumOO(1,j)==3 ObsODat(t).Obs_RangeP1(1,k)=stempNum; elseifHeadODat.SumOO(1,j)==4 ObsODat(t).Obs_RangeP2(1,k)=stempNum; else continue; end end %完成一个卫星的所有观测数据存储 end %完成一个历元的数据存储 end %完成所有历元的数据存储 head=HeadODat; obs=ObsODat; return functionEphDat=ReadGpsData globalEphDat EphDat=struct; [filename,pathname]=uigetfile('*.**N','读取GPS广播星历文件'); fid1=fopen(strcat(pathname,filename),'rt'); if(fid1==-1) msgbox('InputFileorPathisnotcorrect','warning','warn'); return; end while (1) temp=fgetl(fid1); header=findstr(temp,'ENDOFHEADER'); if(~isempty(header)) break; end end i=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)%删除重复数据 fork=i-1: i if(EphDat(i-1).PRN~=EphDat(i).PRN) break; elseifabs(EphDat(i-1).toc-EphDat(i).toc)<0.001 i=i-1; end end end i=i+1; end functionDDDW formatlong clearall tic globalHeadODat; globalObsODat; globalEphDat; %先读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); fork=1: epochNUM tr=ObsODat(k).TimeOEp; %历元接收数据时间 Snum=ObsODat(1,k).SatSum;%该历元观测到的卫星数 ifSnum<4 break;%去除无解的历元 end Code=ObsODat(k).SatCode; %该历元观测到的卫星号组 R=ObsODat(k).Obs_RangeC1;%取出C1观测值,列向量 %卫星发射时间的迭代计算 forj=1: Snum ifR(j)==0 continue; end t=TimetoJD(tr (1),tr (2),tr(3),tr(4),tr(5),tr(6)); t2=mod(t,7)*24*3600;%gpssecond %卫星钟差 dd=-1; fori=1: N if(EphDat(i).PRN==Code(j)&abs(t-EphDat(i).toc)<0.0417*2)%读入时间相近的星历文件 a0=EphDat(i).a0; a1=EphDat(i).a1; a2=EphDat(i).a2; toe=EphDat(i).toe; dt=a0+(t2-toe)*a1+(t2-toe)^2*a2;%卫星钟差 dd=1; break; end end ifdd==-1 msgbox('没有相关星历文件'); return; end tt=tr; ts=tr(6)-R(j)/c;%用秒进行迭代 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)); %卫星位置加地球自转改正 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); ifj==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 ifSnum==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); elseifSnum>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); end end Xp=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--'); toc function[x,y,z]=CalPos(PRN,time) globalEphDat t1=TimetoJD(time (1),time (2),time(3),time(4),time(5),time(6)); t2=TimetoJD(time (1),time (2),time(3),0,0,0); ifisempty(EphDat) EphDat=ReadGpsData; end sz=size(EphDat,2); gg=0; %判断最近的时间 fori=1: sz if(EphDat(i).PRN==PRN&abs(EphDat(i).toc-t1)<0.0417*2) G=EphDat(i); gg=1; break; end end t3=t2-G.weekno*7; %减整
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 利用 MATLAB 编制 GPS 单点 定位 程序