空间后方交会：MATLAB实现与解析

clc
clear
close all

%像点坐标xyz
x=[-0.08615, -0.05340, -0.01478, 0.01046];
y=[-0.06899, 0.08221, -0.07663, 0.06443];
xy=[x',y'];
%地面坐标XYZ
X=[36589.41, 37631.08, 39100.97, 40426.54]';
Y=[25273.32, 31324.51, 24934.98, 30319.81]';
Z=[2195.17, 728.69, 2386.50, 757.31]';
%主距
f=0.15324;
%相片比例尺
a=0.000025; m=50000;
%初始化像空间坐标系原点XS,YS,ZS加快迭代,四个坐标复制成4列的向量
for i=1:size(X,1)
XS(i,1)=sum(X)/4; YS(i,1)=sum(Y)/4; ZS(i,1)=m*f+sum(Z)/4;
end
%初始化其它三个待求量phi w kappa
phi=0; w=0; kappa=0;
%初始化旋转矩阵
R=getR(phi,w,kappa);
%初始化改正数
dx = ones(6,1);
%K为迭代次数
K = 0;

%开始计算
while(abs(dx(1,1))>0.001||abs(dx(2,1))>0.001||abs(dx(3,1))>0.001||abs(dx(4,1))>0.000001||abs(dx(5,1))>0.000001||abs(dx(6,1))>0.000001)
%获取旋转矩阵
R = getR(phi,w,kappa);
%获取共线方程分子分母的数组，每列代表一个坐标的共线方程
X_ba = getX_ba(R,X,Y,Z,XS,YS,ZS);
Y_ba = getY_ba(R,X,Y,Z,XS,YS,ZS);
Z_ba = getZ_ba(R,X,Y,Z,XS,YS,ZS);
%计算所有xy近似值
x1 = -f.*X_ba./Z_ba;
y1 = -f.*Y_ba./Z_ba;
xy1=[x1,y1];
xy1=xy2row(xy1);%xy1整理为x,y交叉存放，便于计算
%计算系数,有几个点就有几个系数阵拼接
A=[];
for i=1:size(x1,1)
    %计算A的前半部分
    A1 = getA1(Z_ba(i,1),R,x1(i,1),y1(i,1),f);
    %计算A的后半部分
    A2 = getA2(x1(i,1),y1(i,1),w, kappa ,f);
    %将每次循环计算好的A拼接起来
    A = [A;A1,A2];
end
%常数项
L = xy2row(xy) - xy1;
%法方程
dx = inv(A'*A)*A'*L;
%外方位改正数,为了计算方便把dx前三个平移量的改正提取出来复制成4列
for i=1:size(X,1)
TX(i,1)=dx(1,1); TY(i,1)=dx(2,1); TZ(i,1)=dx(3,1);
end
%改正数近似求和
XS=XS + TX; YS = YS + TY; ZS = ZS + TZ;
phi =phi + dx(4,1);w =w + dx(5,1); kappa =kappa + dx(6,1);
K=K+1;
end
%求L
%L=xy2row(xy)-;
format longG
disp('Xs  Ys  Zs  φ  ω  κ依次为：')
res=[XS(1,1),YS(1,1),ZS(1,1),phi,w,kappa];
disp(res)

function [res] = xy2row(xy)
%此函数传入xy坐标，将其转换为xy顺序按列排列
res = [];
n=size(xy,1);
for i=1:n
    res=[res;xy(i,1);xy(i,2)];
end

function [Z_ba] = getZ_ba(R,X,Y,Z,XS,YS,ZS)
%此函数传入旋转矩阵和XYZ，XS YS ZS 求得xy共线方程分母Z_ba
%   此处显示详细说明
Z_ba=R(1,3).*(X-XS)+R(2,3).*(Y-YS)+R(3,3).*(Z-ZS);
end

function [X_ba] = getX_ba(R,X,Y,Z,XS,YS,ZS)
%此函数传入旋转矩阵和XYZ，XS YS ZS 求得x共线方程分子X_ba
X_ba=R(1,1).*(X-XS)+R(2,1).*(Y-YS)+R(3,1).*(Z-ZS);
end

function [Y_ba] = getY_ba(R,X,Y,Z,XS,YS,ZS)
%此函数传入旋转矩阵和XYZ，XS YS ZS 求得y共线方程分子Y_ba
Y_ba=R(1,2).*(X-XS)+R(2,2).*(Y-YS)+R(3,2).*(Z-ZS);
end

function [R] = getR(phi,w,kappa)
%此函数传入phi，w，kappa旋转系数，返回旋转矩阵
a1 = cos(phi)* cos(kappa) - sin(phi)* sin(w)* sin(kappa);
a2 = -cos(phi)* sin(kappa) - sin(phi)* sin(w)* cos(kappa);
a3 = -sin(phi)* cos(w);
b1 = cos(w)* sin(kappa);
b2 = cos(w)* cos(kappa);
b3 = -sin(w);
c1 = sin(phi)*cos(kappa) + cos(phi)* sin(w)*sin(kappa);
c2 = -sin(phi)*sin(kappa) + cos(phi)*sin(w)*cos(kappa);
c3 = cos(phi)*cos(w);
R = [a1,a2,a3;
    b1,b2,b3;
    c1,c2,c3];
end

function [res] = getA2(x,y,w,kappa,f)
%此函数传入xy,w,kappa,f,返回A矩阵第二部分A2
a14 = y*sin(w) - (x/f*(x*cos(kappa)-y*sin(kappa))+f*cos(kappa))*cos(w);
a15 = -f*sin(kappa)-x/f*(x*sin(kappa)+y*cos(kappa));
a16 = y;
a24 = -x*sin(w)-(y/f*(x*cos(kappa)-y*sin(kappa))-f*sin(kappa))*cos(w);
a25 = -f*cos(kappa)-y/f*(x*sin(kappa)+y*cos(kappa));
a26 = -x;
res = [a14,a15,a16;
       a24,a25,a26];
end

function [res] = getA1(Z_ba,R,x,y,f)
%此函数传入共线方程分母Y1和旋转矩阵R和xy和主距f，返回A矩阵的前半部分A1
res=zeros(2,3);
for i=1:3
res(1,i)= 1./Z_ba.*(R(i,1)*f+R(i,3)*x);
res(2,i)= 1./Z_ba.*(R(i,2)*f+R(i,3)*y);
end
end