主程序爲：
clc; clear all;close all
warning off
global T_P_E T_M_B P1
P0=[-11 -1 286;%定點S1
-42 -84 255;%定點S2
-80 -71 188;%定點S3
11 -103 186;%定點S4
-85 -22 244;%定點S5
-79 7 155];%定點S6
%x y z
v1=[0.01 0.01 1;%過定點S1的直線向量
0.01 0.01 1;%過定點S2的直線向量
0.01 1 0.01;%過定點S3的直線向量
0.01 1 0.01;%過定點S4的直線向量
1 0.01 0.01;%過定點S5的直線向量
1 0.01 0.01];%過定點S6的直線向量
% x y z
L=-10*(rand(15,6)*2-1);%產生15*3的隨機位移矩陣，定義不同動點到定點P0的距離，負號表示方向
T_P_E=[1 0 0 10;
0 1 0 10;
0 0 1 -50;
0 0 0 1];%被測基準塊座標系在動系中的位姿
T_M_B=[1 0 0 20;
0 1 0 20;
0 0 1 -200;
0 0 0 1];%被測基準塊座標系在動系中的位姿
Q=cell(15,1);%預設元胞數組給Q
for q=1:length(L)
Q{q}=weizijiesuan(P0,v1,L(q,:));%調用位姿解算程序，求解位姿
end
子程序weizijiesuan.m爲：
function Q=weizijiesuan(P0,v1,L)
%已知六個固定點P0，過固定點向量v1，動點到固定點距離L，確定動系在定系中位姿的程序
warning off
global T_P_E T_M_B P1
%% 計算固定點對應的動點
P1(1,:)=P0(1,:)+L(1)*v1(1,:)/(sum(v1(1,:).^2).^(1/2));%計算固定點1對應的動點
P1(2,:)=P0(2,:)+L(2)*v1(2,:)/(sum(v1(2,:).^2).^(1/2));%計算固定點2對應的動點
P1(3,:)=P0(3,:)+L(3)*v1(3,:)/(sum(v1(3,:).^2).^(1/2));%計算固定點3對應的動點
P1(4,:)=P0(4,:)+L(4)*v1(4,:)/(sum(v1(4,:).^2).^(1/2));%計算固定點4對應的動點
P1(5,:)=P0(5,:)+L(5)*v1(5,:)/(sum(v1(5,:).^2).^(1/2));%計算固定點5對應的動點
P1(6,:)=P0(6,:)+L(6)*v1(6,:)/(sum(v1(6,:).^2).^(1/2));%計算固定點6對應的動點
%% 計算六動點確定的三個正交平面
x0=[0,0,1,0,1,0,1,0,0];
x=fsolve(@liudiansanmian,x0);
C1=[x(1) x(2) x(3)];%平面xoy的法向量
C2=[x(4) x(5) x(6)];%平面xoz的法向量
C3=[x(7) x(8) x(9)];%平面yoz的法向量
%% 計算三個正交平面的交點，即被測基準塊的原點
A4=[C1(1),C1(2),C1(3);
C2(1),C2(2),C2(3);
C3(1),C3(2),C3(3)];%求三平面交點的方程的A矩陣
b4=[-1;-1;-1];%求三平面交點的方程的b矩陣
%% 計算被測基準塊在測量系中的座標變換矩陣
P=A4\b4;%三平面交點
NML_C1=[C1(1)/norm(C1) C1(2)/norm(C1) C1(3)/norm(C1)];%平面法向量歸一化，下同
NML_C2=[C2(1)/norm(C2) C2(2)/norm(C2) C2(3)/norm(C2)];
NML_C3=[C3(1)/norm(C3) C3(2)/norm(C3) C3(3)/norm(C3)];
R=[NML_C1(1) NML_C2(1) NML_C3(1);
NML_C1(2) NML_C2(2) NML_C3(2);
NML_C1(3) NML_C2(3) NML_C3(3)];%被測基準塊在測量系中的座標變換矩陣
%% 計算動系在定系中的齊次座標變換矩陣
T_M_E=[R P
0 0 0 1];%被測基準塊在測量系中的齊次座標變換矩陣
T_B_P=(T_M_B)^-1*T_M_E*(T_P_E)^-1;%動系在定系中的齊次座標變換矩陣
%% 計算動系在定系的中位姿
alfa=atan(NML_C2(3)/NML_C3(3));%等價旋轉角alfa
beta=atan(-NML_C1(3)/sqrt(NML_C1(1)^2+NML_C1(2)^2));%等價旋轉角beta
gama=atan(NML_C1(2)/NML_C1(1));%等價旋轉角gama
Q=[T_B_P(1,4) T_B_P(2,4) T_B_P(3,4) alfa beta gama];%程序輸出
子程序的子程序liudiansanmian.m爲
function f=liudiansanmian(x)
%六點求解三正交面法向量的程序，已知量爲六點座標，其中兩點位於同一平面上，兩點位於另一平面上，
%兩點位於第三平面上，待求量爲三個平面的法向量
global T_P_E T_M_B P1
f1=x(1)*P1(1,1)+x(2)*P1(1,2)+x(3)*P1(1,3)+1;
f2=x(1)*P1(2,1)+x(2)*P1(2,2)+x(3)*P1(2,3)+1;
f3=x(4)*P1(3,1)+x(5)*P1(3,2)+x(6)*P1(3,3)+1;
f4=x(4)*P1(4,1)+x(5)*P1(4,2)+x(6)*P1(4,3)+1;
f5=x(7)*P1(5,1)+x(8)*P1(5,2)+x(9)*P1(5,3)+1;
f6=x(7)*P1(6,1)+x(8)*P1(6,2)+x(9)*P1(6,3)+1;
f7=x(1)*x(4)+x(2)*x(5)+x(3)*x(6);
f8=x(1)*x(7)+x(2)*x(8)+x(3)*x(9);
f9=x(4)*x(7)+x(5)*x(8)+x(6)*x(9);
f=[f1;f2;f3;f4;f5;f6;f7;f8;f9];
這裏面T_P_E, T_M_B,P1必須設置爲全局變量，設爲局部變量就不行，不知何故。