定義包含非線性方程組的函數，然後循環利用fsolve函數求解非線性方程組，目的是利用三個已知點確定原點座標

數學模型如前一篇所述，不再累述！

主程序如下：

global XA YA ZA XB YB ZB XC YC ZC %定義全局變量
A=100*(rand(15,9)*2-1);% 設定15組能夠確定座標原點的三個點，每一行代表一個組，每組各元素含義如下：
% XA YA ZA XB YB ZB XC YC ZC
P=zeros(15,3);% 預設內存給被測基準塊原點在測量系中的座標
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);%方程組參數賦值
fun=@myfun_2;
x0=[0;0;192];% 設定初值
opt=optimset('Display','off');%不顯示輸出
x=fsolve(fun,x0,opt); %調用函數
P(i,1)=x(1);%原點X向座標賦值
P(i,2)=x(2);%原點Y向座標賦值
P(i,3)=x(3);%原點Z向座標賦值
end

子程序如下：

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

計算結果如下：

P =

   54.5300  -31.1383   11.6357
  -31.7055   46.5079  -48.9986
   -2.1757  -74.7235   97.1213
  -29.0877  -41.2299  -44.4098
  -66.0507  -30.5754   23.5628
   -5.5375  -13.3490  -92.2326
  -76.5056    1.6898   38.0064
  -81.3499  -47.0950   51.0419
   17.2971  -49.4453  -38.6870
   17.4006  -58.4037  -63.9911
  -38.8896   53.2870   94.6090
    2.2439   -3.2118    9.4896
   -4.3917   29.4629  -82.0220
   78.9970   -3.6448   19.9034
  -23.0974   53.4943  -44.2488

該方法更直觀，更容易理解！