好吧，我将最初的2D解决方案移植到了3D中，然后尝试了您的值…好像我找到了问题。
在3D和3个接收器中，有许多位置具有相同的 值，因此将返回找到的第一个（可能不是您要寻找的位置）。要进行验证，只需为找到的解决方案打印模拟的 值，然后将其与输入的 值进行比较即可。另一个选择是ax.e0在计算之后为最外层循环（在您的情况下）打印最终的优化误差变量，并查看其是否接近零。如果是，则表示近似搜索找到了解决方案…
所以只需添加一个 …这是我更新的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 receivers
double recv[n][4]; // (x,y,z) [m] receiver position,[s] time of arrival of signal
double pos0[3]; // (x,y,z) [m] object's real position
double pos [3]; // (x,y,z) [m] object's estimated position
double v=299800000.0; // [m/s] speed of signal (light)
double err=0.0; // [m] error
double aps_e=0.0; // [m] aprox search error
bool _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
这里预览：
并打印出近似误差变量最终值（以秒为单位的解和输入 时间之间的绝对差之和）…和pos0,pos值…
当心这甚至是不完全安全的，因为它 … ，甚至可能 在圆周上，因为这可能导致 重复…
如果您获得了其他信息（例如知道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()
602392714
清零编程群