连续相对定向 matlab

function RelativeOrientation %相对定向
clear;
clc;
[filenamepathname]=uigetfile（‘14-15.txt‘‘选择相对定向点文件‘）;
fid1=fopen（strcat（pathnamefilename）‘rt‘）;
i=find（‘.‘==filename）;
net_name=filename（1:i-1）;
nt=fscanf（fid1‘%f‘1）;%点对个数
f1=fscanf（fid1‘%f‘1）;%左片
f2=fscanf（fid1‘%f‘1）;%右片
f3=fscanf（fid1‘%f‘[4nt]）;%4行nt列
fclose（fid1）;
x1=f3（1:）‘;%x1
y1=f3（2:）‘;%y1
x2=f3（3:）‘;%x2
y2=f3（4:）‘;%y2
sz=size（y2）;
np=sz（1）;

%确定初始值
f=34.23381118/0.00641;x0=17.35891756;y0=-0.95176349;
phi2=0;omega2=0;kappa2=0;u=0;r=0;
Bu=1000;%Bv=0;Bw=0;
X0=[phi2;omega2;kappa2;u;r];
x1=x1-x0;y1=y1-y0;
x2=x2-x0;y2=y2-y0;

DX=ones（51）;
z=-f*ones（1np）;
ct=0;%循环次数
while （abs（DX（1））>0.0003 || abs（DX（2））>0.0003 || abs（DX（3））>0.0003 || abs（DX（4））>0.0003 || abs（DX（5））>0.0003）
%计算像空辅助坐标
a1=cos（X0（1））*cos（X0（3））-sin（X0（1））*sin（X0（2））*sin（X0（3））;
a2=-cos（X0（1））*sin（X0（3））-sin（X0（1））*sin（X0（2））*cos（X0（3））;
a3=-sin（X0（1））*cos（X0（2））;
b1=cos（X0（2））*sin（X0（3））;
b2=cos（X0（2））*cos（X0（3））;
b3=-sin（X0（2））;
c1=sin（X0（1））*cos（X0（3））+cos（X0（1））*sin（X0（2））*sin（X