A=[-2066241.5001 5360801.8835 2761896.3022

-1983936.0407 5430615.7282 2685375.7214

-1887112.7302 5468749.1944 2677688.9806

-1808505.4212 5512502.2716 2642356.5720

-1847017.0670 5573542.7934 2483802.9904  ];%源坐标

B=[-2066134.4896 5360847.0595 2761895.5970

-1983828.7084 5430658.9827 2685374.6681

-1887005.1714 5468790.6487 2677687.2680

-1808397.7260 5512542.0921 2642354.4550

-1846909.0036 5573582.6511 2483801.6147  ]; %目标坐标

K=[];

for i=1:5

C(:,:,i)=[0 -A(i,3) A(i,2) A(i,1); A(i,3) 0 -A(i,1) A(i,2); -A(i,2) A(i,1) 0 A(i,3)];% 每个点对应的系数

D(:,:,i)=[eye(3) C(:,:,i)];

K=[K;D(:,:,i)];%

end

A1=A';

B1=B';

l=B1(:)-A1(:);

xhat=(K'*K)(K'*l);%系数矩阵【 deltaX deltaY deltaZ  欧拉角x 欧拉角y 欧拉角z  尺度参数】

xhat(4:6)=xhat(4:6)*180*3600/pi;

xhat

v=K*xhat+A1(:)-B1(:)