主程序如下：
%利用不共線三點求解並聯機構動系在定系中的位姿
global XA YA ZA XB YB ZB XC YC ZC T_P_E T_M_B%定義全局變量
A=100*(rand(15,9)*2-1);% 設定15組能夠確定座標原點的三個點，每一行代表一個組，每組各元素含義如下：
% XA YA ZA XB YB ZB XC YC ZC
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];%被測基準塊座標系在動系中的位姿
R=cell(15,1);T_M_E=cell(15,1);T_B_P=cell(15,1);alfa=zeros(15,1);beta=zeros(15,1);gama=zeros(15,1);
Q=cell(15,1);% 預設內存給各變量
for i=1:length(A)
XA=A(i,1); YA=A(i,2);ZA=A(i,3);XB=A(i,4);YB=A(i,5);ZB=A(i,6);XC=A(i,7);YC=A(i,8);ZC=A(i,9);%方程組參數賦值
Q{i}=xuanzhuanjuzhen(A(i,:));%調用子程序，計算被測基準塊在測量系中的旋轉矩陣R
end
子程序xuanzhuanjuzhen.m如下：
function Q=xuanzhuanjuzhen(P)
%利用不共線三點確定被測基準塊在測量系中的旋轉矩陣，輸入A爲1*9向量，即[x1 y1 z1 x2 y2 z2 x3 y3 z3]，輸出R爲3*3矩陣
global XA YA ZA XB YB ZB XC YC ZC T_P_E T_M_B%定義全局變量
%% 利用fsolve函數求解座標原點
fun=@myfun_2;
x0=[0;0;192];% 設定初值
opt=optimset('Display','off');%不顯示輸出
x=fsolve(fun,x0,opt);%調用函數求解座標原點
%% 利用cross函數求三正交向量
X=[XB-XC YB-YC ZB-ZC];%X軸向量,由C點指向B點
b=[XA-XC YA-YC ZA-ZC];%其中一個邊的向量，由C點指向A點
Z=cross(b,X);%Z軸向量，同時垂直於x和b
Y=cross(X,Z);%Y軸向量，同時垂直於x和z
%% 確認向量正負
if X(1)>0
X=X;
else
X=-X;
end
if Y(2)>0
Y=Y;
else
Y=-Y;
end
if Z(3)>0
Z=Z;
else
Z=-Z;
end
%% 向量歸一化
norm_x = sqrt(X(1)^2+X(2)^2+X(3)^2);%X軸向量的模
ux=X(1)/norm_x;
uy=X(2)/norm_x;
uz=X(3)/norm_x;
norm_y = sqrt(Y(1)^2+Y(2)^2+Y(3)^2);%Y軸向量的模
vx=Y(1)/norm_y;
vy=Y(2)/norm_y;
vz=Y(3)/norm_y;
norm_z = sqrt(Z(1)^2+Z(2)^2+Z(3)^2);%Z軸向量的模
wx=Z(1)/norm_z;
wy=Z(2)/norm_z;
wz=Z(3)/norm_z;
%% 計算動系在定系中的齊次座標變換矩陣
R=[ux vx wx;
uy vy wy;
uz vz wz];%%被測基準塊在測量系中的座標變換矩陣
T_M_E=[R x;
0 0 0 1];%被測基準塊在測量系中的齊次座標變換矩陣
T_B_P=(T_M_B)^-1*T_M_E*(T_P_E)^-1;%動系在定系中的齊次座標變換矩陣
%% 計算動系在定系的中位姿
alfa=T_B_P(2,3)/T_B_P(3,3);%等價旋轉角alfa
beta=atan(-T_B_P(1,3)/sqrt(T_B_P(1,1)^2+T_B_P(1,2)^2));%等價旋轉角beta
gama=atan(T_B_P(1,2)/T_B_P(1,1));%等價旋轉角gama
Q=[T_B_P(1,4) T_B_P(2,4) T_B_P(3,4) alfa beta gama];%程序輸出
子程序的子程序如下：
function f=myfun_2(x)
global XA YA ZA XB YB ZB XC YC ZC %定義全局變量
f1=(norm([XA-x(1);YA-x(2);ZA-x(3)]))^2+(norm([XB-x(1);YB-x(2);ZB-x(3)]))^2-(norm([XA-XB;YA-YB;ZA-ZB]))^2;
%勾股定理，原點與其中兩點構成直角三角形
f2=(norm([XB-x(1);YB-x(2);ZB-x(3)]))^2+(norm([XC-x(1);YC-x(2);ZC-x(3)]))^2-(norm([XB-XC;YB-YC;ZB-ZC]))^2;
%勾股定理，原點與其中兩點構成直角三角形
f3=norm([XA-x(1);YA-x(2);ZA-x(3)])+norm([XC-x(1);YC-x(2);ZC-x(3)])-norm([XA-XC;YA-YC;ZA-ZC]);
%三點一線時，中間一點與兩端點距離和等於兩端點之間距離
f=[f1;f2;f3];
end
運行結果如下：
Q =
-92.5236 -8.4133 225.9525 1.6589 -0.2395 -0.8589
29.1866 -44.8791 240.0577 -5.1141 -0.6164 1.1533
35.1826 -80.0305 325.4927 0.7855 0.5133 1.3096
41.0961 29.0553 174.4461 0.2997 -0.5033 0.6695
-88.3679 21.6326 265.1052 0.9142 -0.1099 -1.4699
-67.4241 -29.4202 137.0946 1.7923 -0.0448 -1.0142
-17.0258 -110.2923 224.5796 -4.6028 0.2994 -1.3216
85.5420 18.4292 220.4272 0.4911 -0.7235 -1.2104
-41.8621 -94.1039 254.3527 0.5754 0.5964 -0.0079
-145.5743 0.9559 131.2870 6.8798 0.4713 0.5793
8.2815 15.0953 231.2123 -0.5058 -0.7559 0.2392
-61.4213 53.5875 176.5768 1.0170 0.9116 -0.2903
-119.2199 -131.6504 212.0024 -1.3271 -0.2203 1.1731
-61.6812 -44.2491 285.7788 0.4959 1.1713 0.1633
-48.9099 -54.8524 251.2883 15.6221 0.7180 1.2227
結果有待驗證！！！