问题嵌套近似搜索算法
问题内容：
我从移植的近似搜索算法C++，以Python（逻辑和非常漂亮的原始实现归属于）。然后，我编写了一个脚本来使用该算法来解决二维本地化问题（到达时差问题）。二维解决方案效果很好。但是，当我嵌套到3维时，脚本不会产生预期的本地化。
注意，这几乎不是可解性数学的问题。从理论上讲，只要不是所有接收器都位于一个维空间中，该算法就可以扩展到n给定n+1接收器的任何维度。在这种情况下，我有一个尺寸解决方案的接收器。n+1``n-1``4``3
近似搜索算法可以在这里找到。我已将其从这篇文章中排除了，因为问题几乎肯定与代码的这一部分无关。
我尝试过的
我试着通过pdb一个GUI调试器来逐步解决这个问题。我还尝试过在print语句中执行值检查。不幸的是，部分原因是计算方面发生了 很多
事情，我正在努力准确地确定问题出在哪里。我有一种预感，它可能是与在那里我已经放在ax = Appr...和ay = Appr...，但是这只是一个
预感 （和，我试着放置的多种组合，但没有成功）。
（运行中）二维解决方案：
def localize(recv):
ax = Approximate(0, 5000, 32, 10)
ay = Approximate(0, 5000, 32, 10)
#az = Approximate(0, 5000, 32, 6)
error = 0
dt = [0, 0, 0]
c = 299800000 # Speed of light
baseline = 0
while not ax.done:
ay = Approximate(0, 5000, 32, 10)
while not ay.done:
for i in range(3):
x = recv[i][0] - ax.a
y = recv[i][1] - ay.a
baseline = math.sqrt((x * x) + (y * y))
dt[i] = baseline / c
# Normalize times into deltas from zero
baseline = min(dt[0], dt[1], dt[2])
for j in range(3):
dt[j] -= baseline
error = 0.0
error = 0.0
for k in range(3):
error += math.fabs(recv[k][2] - dt[k])
ay.e = error; ax.e = error
ay.step()
ax.step()
# Found solution
print(ax.aa)
print(ay.aa)
（问题）3维解决方案：
def localize(recv):
ax = Approximate(0, 5000, 32, 10)
ay = Approximate(0, 5000, 32, 10)
az = Approximate(0, 5000, 32, 10)
error = 0
dt = [0, 0, 0, 0]
c = 299800000 # Speed of light
baseline = 0
while not ax.done:
ay = Approximate(0, 5000, 32, 10)
while not ay.done:
az = Approximate(0, 5000, 32, 10)
while not az.done:
for i in range(4):
x = recv[i][0] - ax.a
y = recv[i][1] - ay.a
z = recv[i][2] - az.a
baseline = math.sqrt((x * x) + (y * y) + (z * z))
dt[i] = baseline / c
# Normalize times into deltas from zero
basline = min(dt[0], dt[1], dt[2], dt[3])
for j in range(4):
dt[j] -= basline
error = 0.0
for k in range(4):
error += math.fabs(recv[k][3] - dt[k])
ay.e = error; ax.e = error; az.e = error
az.step()
ay.step()
ax.step()
# Found solution
print(ax.aa)
print(ay.aa)
print(az.aa)
#Localization
# x = 1765 y = 2313 z = 1753
localize([[120,145,90,0.0000002468378075465656],
[20,450,37,0.0000002433879002368936],
[10,-50,324,0.0000002433879002368936],
[67,903,45,0.0000002314851840328957]])
预测的本地化：975.6857619200017，811.0280894080021，1278.482239584
预期的行为：1765、2313、753
（在相当的精度范围内（C++算法提供的精度约为一个单位的分数））。
此外，为乱码表示歉意。需要进行一些精简。
编辑：
就像@jack在下面指出的那样，问题几乎可以肯定是我对错误的计算得出的。不过，我不确定我可能会犯什么错误。它与二维误差计算并没有什么不同，它是求和平方误差问题的最基本的最小化。不知道这是数学问题还是我错过的某些编码问题。
问题答案：
好吧，我将最初的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 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
这里预览：
并打印出近似误差变量最终值（以秒为单位的解和输入 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()
