我已经将近似搜索算法从 C ++ 移植到了 Python (该逻辑和非常好的原始实现可归因于
还打印了近似误差变量最终值(以秒为单位的解和输入的 TDoA 次之间的差的绝对值之和)...和 pos0，pos 值.
请注意，即使这不是完全安全的，因为它有盲点 ... 接收器的高度不应该相同，甚至也不要分布均匀循环，因为这可能导致 TDoA 重复...
如果您获得了其他信息(例如，知道 pos0 在地面上并具有地形图)，则可能可以使用3个不再使用z坐标近似的接收器来执行此操作而是根据 x，y 计算得出的...
PS.您的巢穴上有轻微的化妆品虫子".如您所愿:
ay.e =错误；ax.e =错误；az.e =错误az.step()ay.step()ax.step()我会感到更加安全(但不确定，因为我没有使用Python编写代码):
az.e =错误az.step()ay.e =错误ay.step()ax.e =错误ax.step()I've ported an approximation search algorithm from C++ to Python (the logic and very nice original implementation attributable to). I then wrote a script to use this algorithm in solving a 2-dimensional localization problem (the Time Difference of Arrival problem). The 2-dimensional solution worked great. When I nest to 3-dimensions, however, the script doesn't produce expected localizations.
Note that this almost certainly isn't an issue of the mathematics of solvability. In theory, the algorithm should be extendable to any n dimensions given n+1 receivers, so long as not all n+1 receivers lie in an n-1 dimensional space. In this case, I have 4 receivers for a 3 dimensional solution.
The approximation search algorithm can be found here. I've excluded it from this post as the issue almost certainly doesn't lie with this part of the code.
What I've tried:
I've tried stepping through this with pdb and a GUI debugger. I've also tried sprinkling in print statements to perform value checks. Unfortunately, due in part to the fact that there is so much going on in terms of calculations, I'm struggling to identify precisely where the issue occurs. I have a hunch it may have something to do with where I've placed ax = Appr... and ay = Appr..., however it's only a hunch (and, I've tried many combinations of placement with no success).
(Functioning) 2-Dimensional Solution:
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)(Problematic) 3-Dimensional Solution:
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]])Predicted localization: 975.6857619200017, 811.0280894080021, 1278.482239584
Expected behavior: 1765, 2313, 753
(to within a fair degree of precision (C++ algorithm provides precision on the order of a fraction of a unit)).
Also, apologies for the messy code. It is in need of some streamlining.
EDIT:
As @jack pointed out below, the issue is almost certainly with my calculation of the error. I'm not sure what mistake I could possibly be making, however. It shouldn't be all that different from the 2-dimensional error calculation, it's a very basic minimization of summed squared errors problem. Not sure if it's a mathematical issue, or some coding issue that I've missed.
解决方案Well I ported my original 2D solution into 3D and then tried your values... Looks like I found the problem.
In 3D and 3 receivers there are many positions with the same TDoA values hence the first found is returned (which might not be the one you seek). To verify just print the simulated TDoA values for found solution and compare to your input TDoA values. Another option is to print the final optimization error variable for outer most loop (in your case ax.e0) after computation and see if it is near zero. If it is it means approximation search found solution...
To remedy your case you need at least 4 receivers... so just add one ... Here my updated 3D C++/VCL code:
//--------------------------------------------------------------------------- #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 // stackoverflow/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; } //---------------------------------------------------------------------------So as before just ignore the VCL stuff and port to your environment/language... Its configured so it does not compute for too long (less than 1 sec) and the error is <0.01 m
Here preview:
also with printed the approximation error variable final value (abs sum of difference between solution's and input TDoA times in seconds) ... and pos0,pos values...
Beware even this is not fully safe as it has blind spots ... The receivers should not be at the same altitude and also maybe even not distributed evenly on circle because that can cause the TDoA duplicates...
In case you got additional info (like know that pos0 is at ground and have the terrain map) then it might be possible to do this with 3 receivers where the z coordinate will not be approximated anymore but computed from x,y instead...
PS. your nesting has a slight cosmetics "bug" as you have:
ay.e = error; ax.e = error; az.e = error az.step() ay.step() ax.step()I would feel much safer with (not sure though as I do not code in Python):
az.e = error az.step() ay.e = error ay.step() ax.e = error ax.step()
更多推荐
问题嵌套近似搜索算法
发布评论