Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- //---------------------------------------------------------------------------
- // TDoA Time Difference of Arrival
- //---------------------------------------------------------------------------
- const int n=3;
- double recv[n][3]; // (x,y) [m] receiver position,[s] time of arrival of signal
- double pos0[2]; // (x,y) [m] object's real position
- double pos [2]; // (x,y) [m] object's estimated position
- double v=340.0; // [m/s] speed of signal
- double err=0.0; // [m] error between real and estimated position
- //---------------------------------------------------------------------------
- void compute()
- {
- int i;
- double x,y,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]=256.0+(220.0*cos(a));
- recv[i][1]=256.0+(220.0*sin(a));
- }
- pos0[0]=300.0;
- pos0[1]=220.0;
- // 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];
- a=sqrt((x*x)+(y*y)); // distance to receiver
- recv[i][2]=t0+(a/v); // start time + time of travel
- }
- //---------------------------------------------------------
- // normalize times into deltas from zero
- a=recv[0][2]; for (i=1;i<n;i++) if (a>recv[i][2]) a=recv[i][2];
- for (i=0;i<n;i++) recv[i][2]-=a;
- // fit position
- int N=6;
- approx ax,ay;
- double e,dt[n];
- // min, max,step,recursions,&error
- for (ax.init( 0.0,512.0, 32.0 ,N, &e);!ax.done;ax.step())
- for (ay.init( 0.0,512.0, 32.0 ,N, &e);!ay.done;ay.step())
- {
- // simulate measurement -> dt[]
- for (i=0;i<n;i++)
- {
- x=recv[i][0]-ax.a;
- y=recv[i][1]-ay.a;
- a=sqrt((x*x)+(y*y)); // 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][2]-dt[i]);
- }
- pos[0]=ax.aa;
- pos[1]=ay.aa;
- //---------------------------------------------------------
- // compute error
- x=pos[0]-pos0[0];
- y=pos[1]-pos0[1];
- err=sqrt((x*x)+(y*y)); // [m]
- }
- //---------------------------------------------------------------------------
Add Comment
Please, Sign In to add comment
