基于matlab的摄影测量后方交会元素解算

自己做的后方交会元素解算matlab源程序
废话不多说，直接上代码
代码的原始数据都已经直接赋值了，可以再写一点点代码读入excel或者txt

// A code block
%单幅影像后方交会
%auther:NeverForget2020
%time:2020.9.21
close all;clear; clc;
disp('后方交会求解');
x =[-86.15,-53.40,-14.78,10.46];%%input('每次观测的像平面内的x坐标');
y =[-68.99,82.21,-76.63,64.43];%%input('每次观测的像平面内的y坐标');
X =[36589.41,37631.08,39100.97,40426.54];%%input('每次观测的物空间内的x坐标');
Y =[25273.32,31324.51,24934.98,30319.81];%% input('每次观测的物空间内的y坐标');
Z = [2195.17,728.69,2386.50,757.31];%%input('每次观测的物空间内的z坐标');
x=x/1000;y=y/1000;%修正单位
fk=153.24/1000;
m=50000;
limit1=0.001;limit2=0.0000001;%精度
Xs=sum(X)/4;Ys=sum(Y)/4;Zs=fk*m;%待定参数初始值
q=0;w=0;k=0;%外方位角
R = zeros(3,3);%旋转矩阵
dx=ones(6,1);%改正值
H=fk*m;%航高
round=0;%迭代次数
c=[x(1);y(1);x(2);y(2);x(3);y(3);x(4);y(4)];


while(abs(dx(4,1))>limit2||abs(dx(5,1))>limit2||abs(dx(6,1))>limit2)
R(1,1)=cos(q)*cos(k)-sin(q)*sin(w)*cos(k);
R(1,2)=-cos(q)*sin(k)-sin(q)*sin(w)*cos(k);
R(1,3)=-sin(q)*cos(w);
R(2,1)=cos(w)*sin(k);
R(2,2)=cos(w)*cos(k);