利用MATLAB编制的GPS单点定位程序.docx
《利用MATLAB编制的GPS单点定位程序.docx》由会员分享，可在线阅读，更多相关《利用MATLAB编制的GPS单点定位程序.docx（19页珍藏版）》请在冰点文库上搜索。
利用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_（fpath,fname）;
fid1=fopen（O_,'rt'）;
if（fid1==-1）
msgbox（'','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）;
%补充数据长度
ifLs（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;
[]=uigetfile（'*.**N','读取GPS广播星历文件'）;
fid1=fopen（strcat（pathname,）,'rt'）;
if（fid1==-1）
msgbox（'InputPathisnotcorrect','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.1467;%地球自转角速度
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;
%减整周数
t=t3*24*3600+time（4）*3600+time（5）*60+time（6）;
%化为GPS秒
u=3.986004418E+14;
升级会员 