# Trilateration Math Check

I'm trying to create 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 before 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.