matlab 单点定位.rar_GPS 单点定位程序_GPS matlab_gps 单点定位_gps单点定位_matlab 单

global NAV OBS c OMEGAe GM Uh XX YY ZZ c=2.997925*10^8; %光速 GM=3.986004418e+14; %给出万有引力常数 OMEGAe=7.292115*10^(-5); %给出地球自转角速度 （弧度制） Uh=0.0814; %仪器高 XX=-2831733.70685899;YY=4675665.86876075;ZZ=3275369.34750177; %shao站2011年7月19日的真实坐标 %读取数据 NAV= ReadNavfile; [OBS,XYZ]=ReadObsfile; %读取某一历元数据 for epoch=1:size(OBS,2); A=[];L=[];p=[]; %读取历元中的一颗观测卫星数据 dxyz(1)=1;dxyz(2)=1;dxyz(3)=1; Xk=XYZ(1);Yk=XYZ(2);Zk=XYZ(3);CLK_re=0; %给与初值，进行计算卫星位置 while dxyz(1)>1e-3||dxyz(2)>1e-3||dxyz(3)>1e-3 X0=Xk;Y0=Yk; Z0=Zk; CLK_RECV=CLK_re; for alnum=1:size(OBS(epoch).prn,2) %在NAV中寻找最适宜的导航卫星 j=1;comp_mat=[]; for i=1:size(NAV.prn,2); if(NAV.prn(i)==OBS(epoch).prn(alnum)) dtime=abs(NAV.toe(i)-OBS(epoch).t); comp_mat(:,j)=[i;dtime]; j=j+1; end; end; [,ind]=min(comp_mat(2,:)); kk=comp_mat(1,ind); %得到最佳的观测卫星； %求解最佳卫星的最优估计位置 mts=OBS(epoch).C1(alnum)/c; %传播时间初值； t=OBS(epoch).t-mts-CLK_RECV/c; %赋予初始时刻的该颗卫星发送时间；（以后再考虑卫星钟差） while 1 [x,y,z,delt_sat]=NavPos(t,kk,mts); R=sqrt((x-X0)^2+(y-Y0)^2+(z-Z0)^2); ts=R/c; t=OBS(epoch).t-ts-CLK_RECV/c; if abs(ts-mts)<1e-12; break end; mts=ts; end; %最小二乘法，构建系数矩阵 A(alnum,1)=(X0-x)/R;A(alnum,2)=(Y0-y)/R;A(alnum,3)=(Z0-z)/R;A(alnum,4)=1; %进行对流层改正---简化的hopefiled模型 %计算卫星高度角 [X0_N,Y0_E,Z0_U]=XYZ2NEU(X0,Y0,Z0); [x_N,y_E,z_U]=XYZ2NEU(x,y,z); s_agle=asind((z_U-Z0_U)/sqrt(sum([x_N-X0_N,y_E-Y0_E,z_U-Z0_U].^2))); dtrop=2.47/sind(s_agle)+0.0121; L(alnum,1)=OBS(epoch).C1(alnum)-R+c*delt_sat+c*NAV.tgd(kk)+dtrop; %构成权 if s_agle<10; s_agle=s_agle+6.25; end; p(alnum)=sind(s_agle); end; %计算接收机坐标 P=diag(p); Q=inv(A'*P*A); dxyz=Q*A'*P*L; V=A*dxyz-L; sigma0=sqrt(V'*P*V/(alnum-1)); x_sigma=sigma0*Q(1,1); y_sigma=sigma0*Q(2,2); z_sigma=sigma0*Q(3,3); gdop=sqrt(trace(Q)); pdop=sqrt(trace(Q(1:3,1:3))); Xk=X0+dxyz(1);Yk=Y0+dxyz(2);Zk=Z0+dxyz(3);CLK_re=CLK_RECV+dxyz(4); end; %接收机天线相位中心改正 [B,L,~]=XYZ2BLH(X0+dxyz(1),Y0+dxyz(2),Z0+dxyz(3)); [N,E,~]=XYZ2NEU(X0+dxyz(1),Y0+dxyz(2),Z0+dxyz(3)); deta_xyz=[cos(L),-sin(L),0;sin(L),cos(L),0;0,0,1]*[cos(B),0,-sin(B);0,1,0;sin(B),0,cos(B)]*[Uh,0,0]'; %得到此次历元定位的最终结果 Xk(epoch)=X0+dxyz(1)+deta_xyz(1);Yk(epoch)=Y0+dxyz(2)+deta_xyz(2); Zk(epoch)=Z0+dxyz(3)+deta_xyz(3); %此次历元定位与精确坐标之间的区别 dx(epoch)=Xk(epoch)-XX; dy(epoch)=Yk(epoch)-YY; dz(epoch)=Zk(epoch)-ZZ; %历元坐标各个分量的误差； X_sigma(epoch)=x_sigma; Y_sigma(epoch)=x_sigma; Z_sigma(epoch)=x_sigma; GDOP(epoch)=gdop; %此指标反应卫星分布对GPS伪距测量动态单点定位精度的影响； PDOP(epoch)=pdop; %此指标空间位置精度； display(epoch) %反映历元循环的进度 end Mean_X=mean(Xk);Mean_Y=mean(Yk);Mean_Z=mean(Zk); for i=1:epoch X_bias(i)=Xk(i)-Mean_X; Y_bias(i)=Yk(i)-Mean_Y; Z_bias(i)=Zk(i)-Mean_Z; end; %每个方向的坐标中误差： for i=2:epoch RMS_Mean_X(i-1)=sqrt(sum(X_bias(1:i).^2)/(i-1)); RMS_Mean_Y(i-1)=sqrt(sum(Y_bias(1:i).^2)/(i-1)); RMS_Mean_Z(i-1)=sqrt(sum(Z_bias(1:i).^2)/(i-1)); end; xlswrite('RMS.xlsx',[RMS_Mean_X;RMS_Mean_Y;RMS_Mean_Z]); xlswrite('dxyz.xlsx',[dx;dy;dz]); xlswrite('sigmaXYZ.xlsx',[X_sigma;Y_sigma;Z_sigma]); xlswrite('GDOP.xlsx',GDOP); xlswrite('PDOP.xlsx',PDOP);