好吧，我将最初的2D解决方案移植到了3D中，然后尝试了您的值…好像我找到了问题。
在3D和3个接收器中，有许多位置具有相同的 TDoA
值，因此将返回找到的第一个（可能不是您要寻找的位置）。要进行验证，只需为找到的解决方案打印模拟的 TDoA 值，然后将其与输入的 TDoA
值进行比较即可。另一个选择是
ax.e0在计算之后为最外层循环（在您的情况下）打印最终的优化误差变量，并查看其是否接近零。如果是，则表示近似搜索找到了解决方案…
为了解决您的问题，您至少需要4个接收器… 所以只需添加一个 接收器…这是我更新的3D C ++ / VCL代码：
//---------------------------------------------------------------------------#include <vcl.h>#include <math.h>#pragma hdrstop#include "Unit1.h"#include "backbuffer.h"#include "approx.h"//---------------------------------------------------------------------------#pragma package(smart_init)#pragma resource "*.dfm"TForm1 *Form1;backbuffer scr;//---------------------------------------------------------------------------// TDoA Time Difference of Arrival// https://stackoverflow.com/a/64318443/2521214//---------------------------------------------------------------------------const int n=4; // number of receiversdouble recv[n][4]; // (x,y,z) [m] receiver position,[s] time of arrival of signaldouble pos0[3]; // (x,y,z) [m] object's real positiondouble pos [3]; // (x,y,z) [m] object's estimated positiondouble v=299800000.0; // [m/s] speed of signal (light)double err=0.0; // [m] errordouble aps_e=0.0; // [m] aprox search errorbool _recompute=true;//---------------------------------------------------------------------------void compute() { int i; double x,y,z,a,da,t0; //--------------------------------------------------------- // init positions da=2.0*M_PI/(n); for (a=0.0,i=0;i<n;i++,a+=da) { recv[i][0]=2500.0+(2200.0*cos(a)); recv[i][1]=2500.0+(2200.0*sin(a)); recv[i][2]=500.0*sin(a); } // simulate measurement t0=123.5; // some start time for (i=0;i<n;i++) { x=recv[i][0]-pos0[0]; y=recv[i][1]-pos0[1]; z=recv[i][2]-pos0[2]; a=sqrt((x*x)+(y*y)+(z*z)); // distance to receiver recv[i][3]=t0+(a/v); // start time + time of travel } //--------------------------------------------------------- // normalize times into deltas from zero a=recv[0][3]; for (i=1;i<n;i++) if (a>recv[i][3]) a=recv[i][3]; for (i=0;i<n;i++) recv[i][3]-=a; // fit position int N=8; approx ax,ay,az; double e,dt[n]; // min, max, step,recursions,&error for (ax.init( 0.0,5000.0,200.0, N, &e);!ax.done;ax.step()) for (ay.init( 0.0,5000.0,200.0, N, &e);!ay.done;ay.step()) for (az.init( 0.0,5000.0, 50.0, N, &e);!az.done;az.step()) { // simulate measurement -> dt[] for (i=0;i<n;i++) { x=recv[i][0]-ax.a; y=recv[i][1]-ay.a; z=recv[i][2]-az.a; a=sqrt((x*x)+(y*y)+(z*z)); // distance to receiver dt[i]=a/v; // time of travel } // normalize times dt[] into deltas from zero a=dt[0]; for (i=1;i<n;i++) if (a>dt[i]) a=dt[i]; for (i=0;i<n;i++) dt[i]-=a; // error e=0.0; for (i=0;i<n;i++) e+=fabs(recv[i][3]-dt[i]); } pos[0]=ax.aa; pos[1]=ay.aa; pos[2]=az.aa; aps_e=ax.e0; // approximation error variable e for best solution //--------------------------------------------------------- // compute error x=pos[0]-pos0[0]; y=pos[1]-pos0[1]; z=pos[2]-pos0[2]; err=sqrt((x*x)+(y*y)+(z*z)); // [m] }//---------------------------------------------------------------------------void draw() { scr.cls(clBlack); int i; const double pan_x=0.0; const double pan_y=0.0; const double zoom=512.0/5000.0; double x,y,r=8.0; #define tabs(x,y){ x-=pan_x; x*=zoom; y-=pan_y; y*=zoom; } #define trel(x,y){ x*=zoom; y*=zoom; } scr.bmp->Canvas->Font->Color=clYellow; scr.bmp->Canvas->TextOutA(5, 5,AnsiString().sprintf("Error: %.3lf m",err)); scr.bmp->Canvas->TextOutA(5,25,AnsiString().sprintf("Aprox error: %.20lf s",aps_e)); scr.bmp->Canvas->TextOutA(5,45,AnsiString().sprintf("pos0 %6.1lf %6.1lf %6.1lf m",pos0[0],pos0[1],pos0[2])); scr.bmp->Canvas->TextOutA(5,65,AnsiString().sprintf("pos %6.1lf %6.1lf %6.1lf m",pos [0],pos [1],pos [2])); // recv scr.bmp->Canvas->Pen->Color=clAqua; scr.bmp->Canvas->Brush->Color=clBlue; for (i=0;i<n;i++) { x=recv[i][0]; y=recv[i][1]; tabs(x,y); scr.bmp->Canvas->Ellipse(x-r,y-r,x+r,y+r); } // pos0 scr.bmp->Canvas->Pen->Color=TColor(0x00202060); scr.bmp->Canvas->Brush->Color=TColor(0x00101040); x=pos0[0]; y=pos0[1]; tabs(x,y); scr.bmp->Canvas->Ellipse(x-r,y-r,x+r,y+r); // pos scr.bmp->Canvas->Pen->Color=clYellow; x=pos[0]; y=pos[1]; tabs(x,y); scr.bmp->Canvas->MoveTo(x-r,y-r); scr.bmp->Canvas->LineTo(x+r,y+r); scr.bmp->Canvas->MoveTo(x+r,y-r); scr.bmp->Canvas->LineTo(x-r,y+r); scr.rfs(); #undef tabs(x,y){ x-=pan_x; x*=zoom; y-=pan_y; y*=zoom; } #undef trel(x,y){ x*=zoom; y*=zoom; } }//---------------------------------------------------------------------------__fastcall TForm1::TForm1(TComponent* Owner):TForm(Owner) { Randomize(); pos0[0]=2000.0; pos0[1]=2000.0; pos0[2]= 900.0; scr.set(this); }//---------------------------------------------------------------------------void __fastcall TForm1::FormPaint(TObject *Sender) { draw(); }//---------------------------------------------------------------------------void __fastcall TForm1::tim_updateTimer(TObject *Sender) { if (_recompute) { compute(); _recompute=false; } draw(); }//---------------------------------------------------------------------------void __fastcall TForm1::FormClick(TObject *Sender) { pos0[0]=scr.mx1*5000.0/512.0; pos0[1]=scr.my1*5000.0/512.0; pos0[2]=Random()*1000.0; _recompute=true; }//---------------------------------------------------------------------------因此，像以前一样，只需忽略VCL内容和您的环境/语言的端口…对其进行了配置，因此它的计算时间不会太长（小于
1 sec），并且错误是
<0.01 m
这里预览：
并打印出近似误差变量最终值（以秒为单位的解和输入 TDoA 时间之间的绝对差之和）…和
pos0,pos值…
当心这甚至是不完全安全的，因为它 有盲点… 接收器不应处于相同的高度 ，甚至可能 不能均匀分布 在圆周上，因为这可能导致
TDoA 重复…
如果您获得了其他信息（例如知道
pos0在地面上并具有地形图），则可能可以使用3个接收器来执行此操作，其中不再会近似z坐标，而是从中计算z坐标
x,y…
PS。您的嵌套处有轻微的化妆品“虫”，如下所示：
ay.e = error; ax.e = error; az.e = error az.step() ay.step() ax.step()
我会感到更安全（但不确定，因为我不使用Python编写代码）：
az.e = error az.step() ay.e = error ay.step() ax.e = error ax.step()
