Trilateration Math check

Question

I'm trying to crate an indoor trilateration program working off of N BLE objects. Where I have any number of beacons in a room and my ipad can track its location within them to a sub meter degree of accuracy (I hope).

I asked this question on the GIS overflow board (where I'll also be putting this question) to check over my original Trilateration program. The only comment I got took me to this paper

Right now I'm trying to implement their iterative trilateration math in my code. Their formula is a follows (I am really sorry, I don't know how to put up the proper math formula):

Reference points = Xi, Yi

distance = di

Trivial initial estimate = Xe, Ye

Error in estimated distance = |fi | = di − Math.SquareRoot ((xi − xe )^2 + (yi − ye )^2)

Next they apply a delta to the initial estimate of Xe and Ye as follows:

Xe = Xe + 0.05 Delta x

Ye = Ye + 0.05 Delta y

With this new Delta, they then modified the original fi equation to look like this:

|fi | = di − Math.SquareRoot ((xi − xe )^2 + (yi − ye )^2) / di

Now, reading all that I've written my code as follows:

public GameObject refPoint1, refPoint2;
public float d = 0.0f;
public float f1 = 0.0f;
public float xe, ye = 0.0f;
public float multiplier = 0.05f;

// Use this for initialization
void Start () {

}

// Update is called once per frame
void Update () 
{
    xe = xe + multiplier * Math.Abs(refPoint1.transform.position.x);
    ye = ye + multiplier* Math.Abs(refPoint1.transform.position.y);
    d = Vector3.Distance(refPoint1.transform.position, refPoint2.transform.position);
    f1 = d - (Mathf.Sqrt((refPoint1.transform.position.x - xe) * (refPoint1.transform.position.x - xe) + 
                         (refPoint1.transform.position.y - ye) * (refPoint1.transform.position.y - ye))) / d;

}

In my unity scene I have placed the above code onto each of my three spheres twice so that each sphere is able to reference the other two. When I run this, however, my xe, ye and f1 just continue to grow and grow.

I'm just wondering if I've done something wrong, maybe misinterpreted something I shouldn't have? Any insight on this would be gratefully received.

Shouldn't that line for f1 read `f1 = d - (Mathf.Sqrt((refPoint1.transform.position.x - xe) * (refPoint1.transform.position.x - xe) + (refPoint1.transform.position.y - ye) * (refPoint1.transform.position.y - ye))) / d;` (your last calculation was outside of the sqrt method). — rene, Apr 11 '14 at 11:27
Ah, thank you for the spot. I amended my code but the issue still arises. — N0xus, Apr 11 '14 at 12:12
How is your abs(x) the same as Delta x? That doesn't follow. — Bart, Apr 11 '14 at 14:50
I'm not sure, I'll retry it. To be honest, the whole paper was very confusing with some points. — N0xus, Apr 11 '14 at 15:27

score 0 · Answer 1 · answered Apr 12 '14 at 06:28

They are using first the absolute error to describe the method (Gauss-Newton) in some detail. Then they hint that they used the relative error, which they call for unknown reasons fractional error, to do the actual computations presented in the paper.

Denote

gi(x,y)=sqrt(sqr(x-xi)+sqr(y-yi))

then the absolute error is the absolute value of

di-gi(x,y)

and the relative error is the absolute value of

(di-gi(x,y))/di = 1-gi(x,y)/di

In the formula for B, you have to divide each row by di if you use the relative error as fi.

Trilateration Math check

1 Answers1