function [X0 X2]=readlevelnetdata
global pathname net_name s_datafile b_datafile;
global x y X Y Z f a;
[filename,pathname]=uigetfile('*.txt','input filename');
fid1=fopen(strcat(pathname,filename,s_datafile),'rt');
if(fid1==-1)
msgbox('no correct file');
return;
end
%open a file to read==================================================
surveydat=fscanf(fid1,'%f',[5,4]);
surveydat1=surveydat';
x=surveydat1 (:,1)./1000;%像点坐标x
y=surveydat1(:,2)./1000;%像点坐标y
Y=surveydat1(:,3);%地面坐标X
X=surveydat1(:,4);%地面坐标Y
Z=surveydat1(:,5);%地面坐标Z
f=fscanf(fid1,'%f',1)/1000;
%读取文件============================
X0=ones(6,1);
m=sqrt(((X(1)-X(2))^2+(Y(1)-Y(2))^2)/((x(1)-x(2))^2+(y(1)-y(2))^2));
X0(1,1)=1/4*sum(Y);%Xs
X0(2,1)=1/4*sum(X);%Ys
X0(3,1)=m*f;%Zs
X0(4,1)=0;%v
X0(5,1)=0;%w
X0(6,1)=0;%k
X0
%确定未知数初始值 X0=[Xs;Ys;Zs;v;w;k];===================
% X1=ones(6,1);%改正值初始值值全部为1
n=0;%迭代的次数初始化为0次
l=ones(8,1);
%A=ones(2,6,3);
X2=ones(6,1);%改正值初始值值全部为1
while find(X2>0.0001) %改正值中任意一个值大于0.00001时候,进行迭代
A=ones(2,6,4);
n=n+1
for j=1:4
xy(2*j-1,1)=x(j);
xy(2*j,1)=y(j);
end
%观测值xy==============================================================
a1=cos(X0(4,1))*cos(X0(6,1))-sin(X0(4,1))*sin(X0(5,1))*sin(X0(6,1));
a2=-cos(X0(4,1))*sin(X0(6,1))-sin(X0(4,1))*sin(X0(5,1))*cos(X0(6,1));
a3=-sin(X0(4,1))*cos(X0(5,1));
b1=cos(X0(5,1))*sin(X0(6,1));
b2=cos(X0(5,1))*cos(X0(6,1));
b3=-sin(X0(5,1));
c1=sin(X0(4,1))*cos(X0(6,1))+cos(X0(4,1))*sin(X0(5,1))*sin(X0(6,1));
c2=-sin(X0(4,1))*sin(X0(6,1))+cos(X0(4,1))*sin(X0(5,1))*cos(X0(6,1));
c3=cos(X0(4,1))*cos(X0(5,1));
%计算R阵==============================================================
for i=1:4
x0(i)=-f*(a1*(X(i)-X0(1,1))+b1*(Y(i)-X0(2,1))+c1*(Z(i)-X0(3,1)))/(a3*(X(i)-X0(1,1))+b3*(Y(i)-X0(2,1))+c3*(Z(i)-X0(3,1))); %x(i)值近似值
y0(i)=-f*(a2*(X(i)-X0(1,1))+b2*(Y(i)-X0(2,1))+c2*(Z(i)-X0(3,1)))/(a3*(X(i)-X0(1,1))+b3*(Y(i)-X0(2,1))+c3*(Z(i)-X0(3,1))); %y(i)值近似值
X1=a1*(X(i)-X0(1,1))+b1*(Y(i)-X0(2,1))+c1*(Z(i)-X0(3,1));
Y1=a2*(X(i)-X0(1,1))+b2*(Y(i)-X0(2,1))+c2*(Z(i)-X0(3,1));
Z1=a3*(X(i)-X0(1,1))+b3*(Y(i)-X0(2,1))+c3*(Z(i)-X0(3,1));
A(1,1,i)=1/X0(3,1)*(a1*f+a3*x(i));
A(1,2,i)=1/X0(3,1)*(b1*f+b3*x(i));
A(1,3,i)=1/X0(3,1)*(c1*f+c3*x(i));
A(1,4,i)=y(i)*sin(X0(5,1))-(x(i)/f*(x(i)*cos(X0(6,1))-y(i)*sin(X0(6,1)))+f*cos(X0(6,1)))*cos(X0(5,1));
A(1,5,i)=-f*sin(X0(6,1))-x(i)/f*(x(i)*sin(X0(6,1))+y(i)*cos(X0(6,1)));
A(1,6,i)=y(i);
%=========================================================================
A(2,1,i)=1/X0(3,1)*(a2*f+a3*x(i));
A(2,2,i)=1/X0(3,1)*(b2*f+b3*x(i));
A(2,3,i)=1/X0(3,1)*(c2*f+c3*x(i));
A(2,4,i)=x(i)*sin(X0(5,1))-(y(i)/f*(x(i)*cos(X0(6,1))-y(i)*sin(X0(6,1))-f*sin(X0(6,1))))*cos(X0(5,1));
A(2,5,i)=-f*cos(X0(6,1))-y(i)/f*(x(i)*sin(X0(6,1))+y(i)*cos(X0(6,1)));
A(2,6,i)=-x(i);
%线性化近似值改正数系数阵A==================================================
l(2*i-1,1)=xy(2*i-1)-x0(i);
l(2*i,1)=xy(2*i)-y0(i);
end
l
A=[A(:,:,1);A(:,:,2);A(:,:,3);A(:,:,4)]
X2=inv(A'*A)*A'*l
V=A*X2-l
xy=xy+V
for h=1:4
x(h)=xy(2*h-1,1);
y(h)=xy(2*h,1);
end
X0=X0+X2
end
return
fclose('all');
end
评论0