定义包含非线性方程组的匿名函数，然后循环利用fsolve函数求解非线性方程组，目的是利用三个已知点确定原点坐标

三点确定被测基准块原点在测量系中坐标是位姿测量与解算方法之一，其数学模型如下图所示：

由勾股定理可知：

AP^2+BP^2=AB^2       (1)

BP^2+CP^2=BC^2       (2)

由图可知，显然有：

AP+CP=AC        (2)

A,B,C三点坐标已知，三式联立，可得被测基准块坐标原点P在测量系中的坐标。

若进行了多次位姿测量，则待测坐标原点P不是一个，而是若干个，需调用for循环，依次求出各原点的坐标。

 程序如下：

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)
fun=@(x)([(norm([A(i,1)-x(1);A(i,2)-x(2);A(i,3)-x(3)]))^2+(norm([A(i,4)-x(1);A(i,5)-x(2);A(i,6)-x(3)]))^2-(norm([A(i,1)-A(i,4);A(i,2)-A(i,5);A(i,3)-A(i,6)]))^2,...
    (norm([A(i,4)-x(1);A(i,5)-x(2);A(i,6)-x(3)]))^2+(norm([A(i,7)-x(1);A(i,8)-x(2);A(i,9)-x