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

function t=TimetoJD(Y,M,D,h,f,s)

if(Y>=80)

Y=Y+1900;

else

Y=Y+2000;

end

if M<=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 :a structure stores header information if o-file

% .ApproXYZ[3]; //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

% .SumOO[SumOType]; //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

% .TimeOEpp[2]; //reciever time of epoch

% .SatSum; //number of satellites

% .SatCode[SatSum]; //satellites' PRN

% .Obs_FreL1[SatSum];

% .Obs_FreL2[SatSum];

% .Obs_RangeC1[SatSum];

% .Obs_RangeP1[SatSum];

% .Obs_RangeP2[SatSum];

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(t<100)

s=fgets(fid1);

t=t+1;

L=size(s,2);

if L<81

s(L+1:81)=' ';

end

instrS=s(61:81);

%测站点近似坐标

if strncmp(instrS,'APPROX POSITION XYZ',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));

%历元间隔;

elseif strncmp(instrS,'INTERVAL',8)

HeadODat.interval=str2num(s(5:11));

%测站点号

elseif strncmp(instrS,'MARKER NAME',11)

HeadODat.SiteName=s(1:4)

%天线中心改正

elseif strncmp(instrS,'ANTENNA: DELTA 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));

%第一个历元时间

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=str2num(s(31:42));

HeadODat.TimeOB=TimetoJD(year,month,day,hour,minute,second);

%最后一个历元时间

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=TimetoJD(year,month,day,hour,minute,second);

%观测值类型

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')

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

end

%头文件结束

elseif strncmp(instrS,'END OF HEADER',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);

for k=1:ObsODat(t).SatSum

f=s(31+k*3:32+k*3);

ObsODat(t).SatCode(1,k)=str2num(f);

end

%每个历元的观测数据，按卫星号先后顺序分行存

for k=1:ObsODat(t).SatSum

s=fgets(fid1);

%判断一个卫星的观测数据是否分两行存储，如果为两行，则再读入一行if HeadODat.SumOType>5

sg=fgets(fid1);

s=strcat(s,sg);

end

L=size(s,2);

%补充数据长度

if L

s(L+1:HeadODat.SumOType*16)=' ';

end

%对观测数据判断其类型，并存储到相应的数组中

for j=1:HeadODat.SumOType

stemp=s(j*16-15:j*16);

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 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=ReadGpsData

global EphDat

EphDat=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;

end

while(1)

temp=fgetl(fid1);

header=findstr(temp,'END OF HEADER');

if(~isempty(header))

break;

end

end

i=1;

while(1)

temp=fgetl(fid1);

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) %删除重复数据

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;

end

function DDDW

format long

clear all

tic

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 Snum<4

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)<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

if dd==-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);

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);

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)

global EphDat

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);

if isempty(EphDat)

EphDat=ReadGpsData;

end

sz=size(EphDat,2);

gg=0;

%判断最近的时间

for i=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; %地球引力常数

we=7.2921151467E-5; %地球自转速度

a=G.sqa^2; %地球长半轴

n0=sqrt(u/a^3); %计算的平运动

tk=t-G.toe; %从参考历元开始的时间

if tk>=302400

tk=tk-604800;

elseif tk<-302400

tk=tk + 604800;

end

n=n0+G.dn; %改正后的运动

Mk=G.M0+n*tk;

Ek=Mk; %偏近点角弧度for i=1:4

Ek=Mk+G.e*sin(Ek);

end

fk=2*atan((sqrt((1+G.e)/(1-G.e))*tan(Ek/2))); %真近点角

if(fk<0)

fk=fk+2*pi;

end

cosf=(cos(Ek)-G.e)/(1-G.e*cos(Ek));

sinf=(sqrt(1-G.e^2)*sin(Ek))/(1-G.e*cos(Ek));

uk=fk+G.w; %升交角矩及轨道摄动改正项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; %改正后的升交点经度

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];

%打印结果

% x=num2str(x,'%12.6f');

% y=num2str(y,'%12.6f');

% z=num2str(z,'%12.6f');

% fprintf('卫星号PRN:%2.0f',PRN); fprintf('\n');

% fprintf('时间time:%4.0f%4.0f%4.0f%4.0f%4.0f%4.1f',time); fprintf('\n');

% fprintf('所求卫星在地心坐标系中的空间坐标为：\n');

% fprintf('X = ');fprintf(x,'\n');fprintf('m\n');

% fprintf('Y = ');fprintf(y,'\n');fprintf('m\n');

% fprintf('Z = ');fprintf(z,'\n');fprintf('m\n');

%

return

function EAB=CAL2POL(X,Y,Z,XB,YB,ZB)

format long

L=atan(Y/X);

R=sqrt(X*X+Y*Y+Z*Z);

a=6378137;

e=sqrt(0.0066943799013);

seta=atan(Z/sqrt(X*X+Y*Y));

B=asin(sqrt((a*a-R*R*cos(seta)*cos(seta))/(a*a-R*R*cos(seta)*cos(seta)*e*e)))

B1=atan(tan(seta)*(1+a*e*e*sin(B)/Z/sqrt(1-e*e*sin(B)*sin(B))));

while abs(B1-B)>0.01*pi/180/3600

B=B1;

B1=atan(tan(seta)*(1+a*e*e*sin(B)/Z/sqrt(1-e*e*sin(B)^2)));

end

H=R*cos(seta)/cos(B)-(a/sqrt(1-e*e*sin(B)*sin(B)));

L=L*180/pi;

B=B*180/pi;

DX=XB-X;

DY=YB-Y;

DZ=ZB-Z;

NB=-sin(B)*cos(L)*DX-sin(B)*sin(L)*DY+cos(B)*DZ; EB=-sin(L)*DX+cos(L)*DY;

UB=cos(B)*cos(L)*DX+cos(B)*sin(L)*DY+sin(B)*DZ; SAB=sqrt(NB*NB+UB*UB+EB*EB);

EAB=asin(UB/SAB);