GPS计算例题

if k2<=m_knPnumber% k2点系数已知，该系数为0 B(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;

for i=1:m_Lnumber*3 for j=1:3

D(i,3*k+j)=a(i,j); end

if i/3==fix(i/3) k=k+1; end end

% 将D赋值给D1 for i=1:m_Lnumber*3 D1(i,i)=D(i,i); end

%求权阵P

D2=(D+D'-D1)/(0.00298^2); for i=1:m_Lnumber

P((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; %求解未知点坐标

for i=1:m_Pnumber-m_knPnumber

XYZ0(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); end

6

XYZ0(1)= XYZ(1); XYZ0(2)= XYZ(2); XYZ0(3)= XYZ(3); for i=1:m_Lnumber

k1=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('computing correctly') %输出数据到txt

[Filename pathName] = uiputfile('*.txt','saving file as '); if isequal(Filename,0) || isequal(pathName,0), return; end

str=[pathName Filename]; fid2=fopen(str,'wt'); fprintf(fid2,' total points: %d\\n knownPnumber:%d\\n Numdirectionvalues:%d\\n',m_Pnumber,m_knPnumber,m_Lnumber); fprintf(fid2,'\\n known Pcoordinates\\n'); %已知点坐标

for i=1:m_knPnumber

fprintf(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 = = = = coordinate adjustment and prcision = = = = \\n');

fprintf(fid2,'\\nNo. P X Y Z mx my mz M\\n'); for i=1:m_Pnumber xi=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- %3s',i,pname{i});

fprintf(fid2,'.4f .4f .4f',xi,yi,zi); m1=sqrt(Q1(3*i-2,3*i-2))*u;

7

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

fprintf(fid2,' = = = =The coordinate difference adjustment and prcision= = = = \\n');

fprintf(fid2,'\\n P1 P2 xx yy zz Vx Vz xx+Vx yy+Vy zz+Vz M'); for i=1:m_Lnumber k1=dir1(i); k2=dir2(i);

fprintf(fid2,'\\n\\n%6s',pname{k1}); fprintf(fid2,'%7s',pname{k2});

fprintf(fid2,'.3f .3f .3f',XX(i),YY(i),ZZ(i)); fprintf(fid2,'.3f .3f .3f',l(3*i-2),l(3*i-1),l(3*i));

fprintf(fid2,'.3f .3f .3f',XX(i)+V1(3*i-2),YY(i)+V1(3*i-1),ZZ(i)+V1(3*i)); Fx=zeros(1,3*m_Pnumber); Fx(1,3*k1-2)=-1; Fx(1,3*k2-2)=1;

if k1<=m_knPnumber Fx(1,3*k1-2)=-1; end

if k2<=m_knPnumber Fx(1,3*k2-2)=1; end

qx=sqrt(Fx*Q1*Fx')*u; Fy=zeros(1,3*m_Pnumber); Fy(1,3*k1-1)=-1; Fy(1,3*k2-1)=1;

if k1<=m_knPnumber Fy(1,3*k1-1)=-1; end

if k2<=m_knPnumber Fy(1,3*k2-1)=1; end

qy=sqrt(Fy*Q1*Fy')*u; Fz=zeros(1,3*m_Pnumber); Fz(1,3*k1-1)=-1; Fz(1,3*k2-1)=1;

if k1<=m_knPnumber

8

Vy Fz(1,3*k1)=-1; end

if k2<=m_knPnumber Fz(1,3*k2)=1; end

qz=sqrt(Fz*Q1*Fz')*u;

fprintf(fid2,'%7.3f',sqrt(qx*qx+qy*qy+qz*qz)); end

fclose(fid2);

msgbox('saving data suceessfully'); 运行结果： total points: 4 knownPnumber:1 Numdirectionvalues:5 known Pcoordinates

LCO1 -1974638.7340 4590014.8190 3953144.9235

= = = = coordinate adjustment and prcision = = = =

No. P X Y Z mx my mz M

1 LCO1 -1974638.7340 4590014.8190 3953144.9235 0.000 0.000 0.000 0.000 2 LCO2 -1973420.1730 4591054.0460 3951407.2035 0.002 0.005 0.004 0.007 3 LCO3 -1974825.7040 4591232.2030 3950235.8235 0.005 0.013 0.011 0.017 4 LCO4 -1974909.1910 4590518.0270 3951265.0005 0.003 0.008 0.007 0.011

= = = =The coordinate difference adjustment and prcision= = = =

P1 P2 xx yy zz Vx Vy Vz xx+Vx yy+Vy zz+Vz M

LCO2 LCO1 -1218.561 -1039.227 1737.720 0.000 0.000 0.000 -1218.561 -1039.228 1737.719 0.008

LCO4 LCO1 270.457 -503.208 1879.923 -0.000 0.000 -0.000 270.454 -503.202 1879.931 0.011

LCO4 LCO2 1489.013 536.030 142.218 0.005 -0.011 -0.015 1489.005 536.047 142.242 0.010

LCO3 LCO2 1405.531 -178.157 1171.380 -0.000 0.000 -0.000 1405.530 -178.155 1171.383 0.017

LCO4 LCO3 83.497 714.153 -1029.199 -0.010 0.023 0.022 83.505 714.134 -1029.215 0.019

9