clear;
clc;
% IGS精密星历星历文件名称
nfilename='igs17761.sp3';
% 读取IGS精密星历数据
[ndata]=freadnfile(nfilename);
% 观测站GPS观测文件名称，要事先进行数据检查，不能有有问题的数据（如观测数据缺失等）
referfile='monp02002.14o';
% 以文本方式（text）打开观测站GPS观测文件
[fidr,message]=fopen(referfile,'rt');
fidr; % if fidn==-1,文件错误
disp('开始读取观测站观测文件表头的内容');
while (feof(fidr)~=1)
liner=fgets(fidr);
disp(liner);
[row,column]=size(liner);
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
% 读取头文件中给出的基站初始坐标
if ((column>=79)&(liner(61:79)=='APPROX POSITION XYZ'))
rx0=str2num(liner(02:14));
ry0=str2num(liner(16:28));
rz0=str2num(liner(30:42));
end
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
if ((column>=73)&(liner(61:73)=='END OF HEADER'))
sitefidr=ftell(fidr);
disp('读取观测站观测表头文件完毕');
break;
end
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
end
% 开始读取观测数据
disp(' ');
disp('开始读取观测数据的内容');
j=1;
stm=eye(4);Q=zeros(4);X=zeros(4,1);P=zeros(4);
X(1,1)=-2386247.37;X(2,1)=-4802359.15;X(3,1)=3444902.35;X(4,1)=3.696798628092e-08;
Y(1,1)=-2386247.37;Y(2,1)=-4802359.15;Y(3,1)=3444902.35;Y(4,1)=3.696798628092e-08;
P(1,1)=5^2;P(2,2)=5^2;P(3,3)=5^2;P(4,4)=100^2;Q(4,4)=3^2;
%MONP -2386347.37 -4802359.15 3444902.35
while (feof(fidr)~=1)
j
%^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^读取观测站观测数据^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^%
liner=fgetl(fidr)
liner(11:12);%% 读取观测时间（第11-12列为时：05）
liner(14:15);%% 读取观测时间（第14-15列为分：35）
liner(17:26);%% 读取观测时间（第17-26列为秒：01.0000000）
epochr(j,1)=str2num(liner(11:12))*60*60+str2num(liner(14:15))*60+str2num(liner(17:26));%% 将观测时间全部划算为秒
svusedr=str2num(liner(31:32));%% 读取观测卫星数（第31-32列为卫星数：04）
epochdatar=zeros(svusedr,13);
for i=1:svusedr
epochdatar(i,1)=str2num(liner((31+i*3):(32+i*3)));%%卫星PRN号
end
lightspeed=2.99792458e+8;
wc=zeros(svusedr,1);
for i=1:svusedr
liner=fgetl(fidr); %%读取下一行数据
epochdatar(i,2)=str2num(liner(67:78));%% C1观测数据
epochdatar(i,3)=str2num(liner(2:14)); %% L1观测数据
epochdatar(i,4)=str2num(liner(18:30));%% L2观测数据
epochdatar(i,5)=str2num(liner(51:62));%% P1码观测数据
epochdatar(i,7)=rx0;
epochdatar(i,8)=ry0;
epochdatar(i,9)=rz0;
wc(i,1)=epochdatar(i,2)/lightspeed;
[rsvx,rsvy,rsvz,rsvbias]=fsvposion(epochr(j,1)-wc(i,1),epochdatar(i,1),ndata);
epochdatar(i,10)=rsvx;
epochdatar(i,11)=rsvy;
epochdatar(i,12)=rsvz;
epochdatar(i,13)=rsvbias;
end
fulldata=epochdatar;
[svused,column]=size(fulldata);
for h=1:svused
fulldata(h,2)=fulldata(h,2)+lightspeed*fulldata(h,13)/10^6;
end
%求卫星坐标与测站概略坐标之间的近似距离
rou=zeros(svused,1);
for h=1:svused
rou(h,1)=sqrt((fulldata(h,10)-fulldata(h,7))^2+(fulldata(h,11)-fulldata(h,8))^2+(fulldata(h,12)-fulldata(h,9))^2);
end
%计算实验所需各时刻卫星高度角
%MONP LAT/LON 32.89194103 -116.42235198
%MONP -2386247.37 -4802359.15 3444902.35
sinB=0.543056;cosB=0.839696;sinL=-0.895538;cosL=-0.444985;
H=[-sinB*cosL,-sinB*sinL,cosB;-sinL,cosL,0;cosB*cosL,cosB*sinL,sinB];
fai=zeros(svused,3);mde=zeros(svused,3);ZS=zeros(svused,3);EL=zeros(svused,1);
for h=1:svused
fai(h,1)=fulldata(h,10)-fulldata(h,7);fai(h,2)=fulldata(h,11)-fulldata(h,8);fai(h,3)=fulldata(h,12)-fulldata(h,9);
end
for h=1:svused
ZS(h,1)=H(1,:)*[fai(h,1);fai(h,2);fai(h,3)];
ZS(h,2)=H(2,:)*[fai(h,1);fai(h,2);fai(h,3)];
ZS(h,3)=H(3,:)*[fai(h,1);fai(h,2);fai(h,3)];
end
% for h=1:svused
% ZS(h,3)=ZS(h,3)+3.1415926
% end
for h=1:svused
EL(h,1)=atan(ZS(h,3)/sqrt(ZS(h,1)^2+ZS(h,2)^2));
end
%MARINI映射函数模型
ad=(1.232+0.0139*cosB-0.0209*986.6717+0.00215*(15+273.15-283))*0.001;
bd=(3.1612-0.16*cosB-0.0331*986.6717+0.00206*(15+273.15-283))*0.001;
cd=(71.244-4.293*cosB-0.149*986.6717-0.0021*(15+273.15-283))*0.001;
aw=(0.583-0.011*cosB-0.052*986.6717+0.0014*(15+273.15-283))*0.001;
bw=(1.402-0.102*cosB-0.101*986.6717+0.002*(15+273.15-283))*0.001;
cw=(45.85-1.91*cosB-1.29*986.6717+0.015*(15+273.15-283))*0.001;
for h=1:svused
mde(h,1)=(1+(ad/(1+(bd/(1+cd)))))/(sin(EL(h,1))+(ad/(sin(EL(h,1))+(bd/(sin(EL(h,1))+cd)))));
mde(h,2)=(1+(aw/(1+(bw/(1+cw)))))/(sin(EL(h,1))+(aw/(sin(EL(h,1))+(bw/(sin(EL(h,1))+cw)))));
mde(h,3)=0.9*mde(h,1)+0.1*mde(h,2);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%地面点位置卡尔曼滤波计算
B=zeros(svused,4);L=zeros(svused,1);RK=zeros(svused,svused)
XT=stm*X(:,j);
bf=zeros(svused,1);
for h=1:svused
bf(h,1)=1/rou(h,1);
end
for h=1:svused
B(h,1)=bf(h,1)*(fulldata(h,10)-fulldata(h,7));B(h,2)=bf(h,1)*(fulldata(h,11)-fulldata(h,8));B(h,3)=bf(h,1)*(fulldata(h,12)-fulldata(h,9));
B(h,4)=299792458;
end
for h=1:svused
L(h,1)=fulldata(h,2)-1/bf(h,1)-2.27*mde(h,3);
end
PT=stm*P*stm'+Q;
for h=1:svused
RK(h,h)=4/((sin(EL(h,1)))^2);
end
M=vpa(B*PT*B');
N=vpa(RK);
R=M+N;
G=eye(h)/R;
K=PT*B'*G;
X(:,j+1)=XT+K*(L+B*Y-B*XT);
format long
P=(eye(4)-K*B)*PT;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
j=j+1;
end
%显示绘图部分，并生成EXCEL表格文件
[row,column]=size(X);
fid=fopen('X.xls','w');
for i=1:column
fprintf(fid,'%d\t %12.9f\t %12.9f\t %12.9f\n', i,X(1,i),X(2,i),X(3,i));
end
fclose(fid);
%%绘制单点定位的残差图
%MONP -2386247.37 -4802359.15 3444902.35
for i=1:column
plotx(i)=X(1,i)+2386247.37;
ploty(i)=X(2,i)+4802359.15;
plotz(i)=X(3,i)-3444902.35;
end
figure(3);
subplot(3,1,1);
hold on;
plot(plotx,'r');
ylabel('error/m');
xlabel('单点定位的X坐标残差图');
figure(3);
subplot(3,1,2);
hold on;
plot(ploty,'r');
ylabel('error/m');
xlabel('单点定位的Y坐标残差图');
figure(3);
subplot(3,1,3);
hold on;
plot(plotz,'r');
ylabel('error/m');
xlabel('单点定位的Z坐标残差图');
% figure(4);
% hold on;
% plot(X(1,:),X(2,:),'*');
% ylabel('平面Y坐标');
% xlabel('平面X坐标');
- 1
- 2
前往页