GPS计算例题

例1图为一简单GPS网，用两台GPS接收机观测，测得5条基线向量，15n，每一个基线向量中三个坐标差观测值相关，由于两台GPS接收机接收机观测，所以各观测基线向量相互独立，网中点1LOC其三维坐标已知，其余三个为待定点，参数个数9t。图1已知点信息（单位：m）LOC1XYZ-1974638.73404590014.81903953144.9235编号起点终点XYZ基线方差阵1LOC2LOC1-1218.561-1039.2271737.7206-1.008592E6-1.109356E7-4.371401E-6-1.339931E7-5.097008E-7-2.320999E称对2LOC4LOC1270.457-5.3.2081879.9236-6.035577E6-5.902876E6-2.319683E-6-6.341291E6-2.396533E-6-1.044894E称对3LOC4LOC21489.013536.030142.2186-3.019233E6-3.069820E6-1.252374E-6-3.362548E6-1.329620E-7-5.850064E称对4LOC3LOC21405.531-178.1571171.3806-4.820125E6-5.480745E6-2.174106E-6-6.858585E6-2.636702E-6-1.205319E称对5LOC4LOC383.497714.153-1029.1995-4.324110E5-4.633565E5-1.971468E-5-5.194777E5-2.175476E-6-9.662657E称对编程代码：%输入基线阵[FilenamepathName]=uigetfile({'*.txt','TxtFiles|*.txt'},'输入基线差');L=length(Filename);ifL5msgbox('wrongFile','FileopenError');endfid=fopen([pathNameFilename]);%打开数据文件tline1=fgetl(fid);str='[-]?\d[\.]?\d*';f1=str2double(regexp(tline1,str,'match'));m_Pnumber=f1(1,1);%总点数m_knPnumber=f1(1,2);%已知点数m_Lnumber=f1(1,3);%总的观测值tline2=fgetl(fid);st='[\w][\w][\w][-]?\d[\.]?\d*';pname=regexp(tline2,st,'match');%提取点名[P1,P2,P3,P4,P5]=textread([pathNameFilename],'%s%s%s%s%s','headerlines',2);%输入基线方差阵[Filename1pathName1]=uigetfile({'*.txt','TxtFiles|*.txt'},'输入基线方差阵');L=length(Filename);ifL5msgbox('wrongFile','FileopenError');enda=textread([pathName1Filename1]);fori=1:m_knPnumber%提取与已知点相关数据pname1{i}=P1{i};point2(i)=str2num(cell2mat(P2(i)));point3(i)=str2num(cell2mat(P3(i)));point4(i)=str2num(cell2mat(P4(i)));endXYZ=zeros(1,3*m_Pnumber);%命名点坐标大小fori=1:m_knPnumberc=strmatch(pname1(i),pname);%将点名转换成点号XYZ(1,3*c-2)=point2(i);XYZ(1,3*c-1)=point3(i);XYZ(1,3*c)=point4(i);end%将未知点的x坐标赋值为10^30fori=1:(m_Pnumber-m_knPnumber)XYZ(1,3*(m_knPnumber+i)-2)=10^30;endfori=1:m_Lnumberdir1(i)=strmatch(P1(i+m_knPnumber),pname);%起点点号dir2(i)=strmatch(P2(i+m_knPnumber),pname);%终点点号XX(i)=str2num(cell2mat(P3(i+m_knPnumber)));%x坐标YY(i)=str2num(cell2mat(P4(i+m_knPnumber)));%y坐标ZZ(i)=str2num(cell2mat(P5(i+m_knPnumber)));%z坐标end%计算未知点近似坐标fori=1:m_Lnumberk1=dir1(i);%起点点号k2=dir2(i);%终点点号ifXYZ(1,3*k1-2)=10^29&XYZ(1,3*k2-2)=10^29%起点已知，终点未知XYZ(1,3*k2-2)=XYZ(1,3*k1-2)+XX(i);XYZ(1,3*k2-1)=XYZ(1,3*k1-1)+YY(i);XYZ(1,3*k2)=XYZ(1,3*k1)+ZZ(i);elseifXYZ(1,3*k1-2)=10^29&XYZ(1,3*k2-2)=10^29%起点未知，终点已知XYZ(1,3*k1-2)=XYZ(1,3*k2-2)-XX(i);XYZ(1,3*k1-1)=XYZ(1,3*k2-1)-YY(i);XYZ(1,3*k1)=XYZ(1,3*k2)-ZZ(i);endend%计算法方程系数fori=1:m_Lnumberk1=dir1(i);k2=dir2(i);B(3*i-2,3*k1-2)=-1;%X相关系数B(3*i-2,3*k2-2)=1;ifk1=m_knPnumber%k1点系数已知，该系数为0B(3*i-2,3*k1-2)=0;endifk2=m_knPnumber%k2点系数已知，该系数为0B(3*i-2,3*k2-2)=0;endB(3*i-1,3*k1-1)=-1;%Y相关系数B(3*i-1,3*k2-1)=1;ifk1=m_knPnumber%k1点系数已知，该系数为0B(3*i-1,3*k1-1)=0;endifk2=m_knPnumber%k2点系数已知，该系数为0B(3*i-1,3*k2-1)=0;endB(3*i,3*k1)=-1;%Z相关系数B(3*i,3*k2)=1;ifk1=m_knPnumber%k1点系数已知，该系数为0B(3*i,3*k1)=0;endifk2=m_knPnumber%k2点系数已知，该系数为0B(3*i,3*k2)=0;end%提取x，y，z的常数项l(3*i-2)=XYZ(3*k2-2)-XYZ(3*k1-2)-XX(i);l(3*i-1)=XYZ(3*k2-1)-XYZ(3*k1-1)-YY(i);l(3*i)=XYZ(3*k2)-XYZ(3*k1)-ZZ(i);end%综合提取B1，p和l矩阵B1=B(:,3*m_knPnumber+1:3*m_Pnumber);D=zeros(3*m_Lnumber,3*m_Lnumber);%基线方差阵k=0;fori=1:m_Lnumber*3forj=1:3D(i,3*k+j)=a(i,j);endifi/3==fix(i/3)k=k+1;endend%将D赋值给D1fori=1:m_Lnumber*3D1(i,i)=D(i,i);end%求权阵PD2=(D+D'-D1)/(0.00298^2);fori=1:m_LnumberP((3*i-2):3*i,(3*i-2):3*i)=inv(D2((3*i-2):3*i,(3*i-2):3*i));end%最小二乘求解dx=inv(B1'*P*B1)*B1'*P*(-l)';%求解协因数阵Q=inv(B1'*P*B1);Q1=zeros(3*m_Pnumber,3*m_Pnumber);Q1(3*m_knPnumber+1:3*m_Pnumber,3*m_knPnumber+1:3*m_Pnumber)=Q;%求解所有点的改正数dX=zeros(1,3*m_Pnumber);dX(1,3*m_knPnumber+1:3*m_Pnumber)=dx;%求解未知点坐标fori=1:m_Pnumber-m_knPnumberXYZ0(1,3*(i+1)-2)=XYZ(1,3*(i+1)-2)+dx(3*i-2);XYZ0(1,3*(i+1)-1)=XYZ(1,3*(i+1)-1)+dx(3*i-1);XYZ0(1,3*(i+1))=XYZ(1,3*(i+1))+dx(3*i);endXYZ0(1)=XYZ(1);XYZ0(2)=XYZ(2);XYZ0(3)=XYZ(3);fori=1:m_Lnumberk1=dir1(i);%起点点号k2=dir2(i);%终点点号V(3*i-2)=XYZ0(3*k2-2)-XYZ0(3*k1-2)-XX(i);V(3*i-1)=XYZ0(3*k2-1)-XYZ0(3*k1-1)-YY(i);V(3*i)=XYZ0(3*k2)-XYZ0(3*k1)-ZZ(i);end%求解基线差的改正数V1=B*dX'-l';%计算单位权中误差u=sqrt((V*P*V')/(m_Pnumber-m_knPnumber));msgbox('computingcorrectly')%输出数据到txt[FilenamepathName]=uiputfile('*.txt','savingfileas');ifisequal(Filename,0)||isequal(pathName,0),return;endstr=[pathNameFilename];fid2=fopen(str,'wt');fprintf(fid2,'totalpoints:%d\nknownPnumber:%d\nNumdirectionvalues:%d\n',m_Pnumber,m_knPnumber,m_Lnumber);fprintf(fid2,'\nknownPcoordinates\n');%已知点坐标fori=1:m_knPnumberfprintf(fid2,'\n%8s',pname{i});fprintf(fid2,'%8.4f%8.4f%8.4f',XYZ(3*i-2),XYZ(3*i-1),XYZ(3*i));end%输出未知点平差值及精度fprintf(fid2,'\n\n\n====coordinateadjustmentandprcision====\n');fprintf(fid2,'\nNo.PXYZmxmymzM\n');fori=1:m_Pnumberxi=XYZ(3*i-2);yi=XYZ(3*i-1);zi=XYZ(3*i);dxi=dX(3*i-2);dyi=dX(3*i-1);dzi=dX(3*i);fprintf(fid2,'\n%2d%3s',i,pname{i});fprintf(fid2,'%14.4f%12.4f%12.4f',xi,yi,zi);m1=sqrt(Q1(3*i-2,3*i-2))*u;fprintf(fid2,'%7.3f',m1);m2=sqrt(Q1(3*i-1,3*i-1))*u;fprintf(fid2,'%7.3f',m2);m3=sqrt(Q1(3*i,3*i))*u;fprintf(fid2,'%7.3f',m3);fprintf(fid2,'%7.3f',sqrt(m1*m1+m2*m2+m3*m3));end%输出基线差及精度fprintf(fid2,'\n\n%8s','')