定义包含非线性方程组的函数，然后循环利用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