BLE (iBeacons) Trilateration

Question

I'm a student at the University of Furtwangen in Germany.

I am in my final term and I am writing my thesis now. I'm very interested in iBeacons and the technology behind them. My current project is to compare the beacon technology with other technologies like GPS, Wireless-location, GSM, and NFC. For my thesis, I will create different use-cases and compare the results.

Over the last few days I've tried to determine my position in a room. I use the relative distance (accuracy) from three beacons and gave every beacon a fixed position in my room. I get three circles and calculate 6 intersections. When a radian (accuracy) is too low I increase this value artificially. Then I look which of the 6 points (intersections) are the nearest. (The three nearest points) With those points I get an triangle, and with this I calculate the middle point.

My problem is that the result is not really the best.

I found a better solution here:

https://gis.stackexchange.com/questions/40660/trilateration-algorithm-for-n-amount-of-points

but I have am having trouble implementing this in Objective C. But I understand the solution. How can I import or get this in Objective C. I found some libs (C, C++) but I'm not really sure which of these libs is the best.

The best solution for me will be a Objectice C math library which can calculate with these points (x1,x2,x3, -- ,y1,y2,y3, --- ,r1,r2,r3).

Graphic of my calculation now

Felid

Have you had any luck with this? – Hatchment

I would be interested in the first solution as well, as I want to experiment a bit with the possibility for a kind of tour guide application. The accuracy is not the most important for me. Is there anyway you can share the logic, or even some of the calculation code? – Barlow

Would you mind sharing how you achieved the iPad app in the graphic that you posted? I've managed to come up with an algorithm based on the wikipedia Trilateration article (en.m.wikipedia.org/wiki/Trilateration) but I'm currently trying to find the best way to map my coordinates in iOS. – Unmoor

Answer 1

I was struggling with the same problem, then I found this solution, written in python. I tried porting the code into objective-c and using the same case for testing and the result is accurate. I modified the code so it can accept 2-dimension vector as well.

The test case was :

P1 = (3,0) r1 = 6.4031
P2 = (9,0) r2 = 4.1231
P3 = (4,8) r3 = 5.6568

I ran this data through the code :

//P1,P2,P3 is the point and 2-dimension vector
NSMutableArray *P1 = [[NSMutableArray alloc] initWithCapacity:0];
[P1 addObject:[NSNumber numberWithDouble:3]];
[P1 addObject:[NSNumber numberWithDouble:0]];


NSMutableArray *P2 = [[NSMutableArray alloc] initWithCapacity:0];
[P2 addObject:[NSNumber numberWithDouble:9]];
[P2 addObject:[NSNumber numberWithDouble:0]];

NSMutableArray *P3 = [[NSMutableArray alloc] initWithCapacity:0];
[P3 addObject:[NSNumber numberWithDouble:4]];
[P3 addObject:[NSNumber numberWithDouble:8]];

//this is the distance between all the points and the unknown point
double DistA = 6.4031;
double DistB = 4.1231;
double DistC = 5.6568;

// ex = (P2 - P1)/(numpy.linalg.norm(P2 - P1))
NSMutableArray *ex = [[NSMutableArray alloc] initWithCapacity:0];
double temp = 0;
for (int i = 0; i < [P1 count]; i++) {
    double t1 = [[P2 objectAtIndex:i] doubleValue];
    double t2 = [[P1 objectAtIndex:i] doubleValue];
    double t = t1 - t2;
    temp += (t*t);
}
for (int i = 0; i < [P1 count]; i++) {
    double t1 = [[P2 objectAtIndex:i] doubleValue];
    double t2 = [[P1 objectAtIndex:i] doubleValue];
    double exx = (t1 - t2)/sqrt(temp);
    [ex addObject:[NSNumber numberWithDouble:exx]];
}

// i = dot(ex, P3 - P1)
NSMutableArray *p3p1 = [[NSMutableArray alloc] initWithCapacity:0];
for (int i = 0; i < [P3 count]; i++) {
    double t1 = [[P3 objectAtIndex:i] doubleValue];
    double t2 = [[P1 objectAtIndex:i] doubleValue];
    double t3 = t1 - t2;
    [p3p1 addObject:[NSNumber numberWithDouble:t3]];
}

double ival = 0;
for (int i = 0; i < [ex count]; i++) {
    double t1 = [[ex objectAtIndex:i] doubleValue];
    double t2 = [[p3p1 objectAtIndex:i] doubleValue];
    ival += (t1*t2);
}

// ey = (P3 - P1 - i*ex)/(numpy.linalg.norm(P3 - P1 - i*ex))
NSMutableArray *ey = [[NSMutableArray alloc] initWithCapacity:0];
double p3p1i = 0;
for (int  i = 0; i < [P3 count]; i++) {
    double t1 = [[P3 objectAtIndex:i] doubleValue];
    double t2 = [[P1 objectAtIndex:i] doubleValue];
    double t3 = [[ex objectAtIndex:i] doubleValue] * ival;
    double t = t1 - t2 -t3;
    p3p1i += (t*t);
}
for (int i = 0; i < [P3 count]; i++) {
    double t1 = [[P3 objectAtIndex:i] doubleValue];
    double t2 = [[P1 objectAtIndex:i] doubleValue];
    double t3 = [[ex objectAtIndex:i] doubleValue] * ival;
    double eyy = (t1 - t2 - t3)/sqrt(p3p1i);
    [ey addObject:[NSNumber numberWithDouble:eyy]];
}


// ez = numpy.cross(ex,ey)
// if 2-dimensional vector then ez = 0
NSMutableArray *ez = [[NSMutableArray alloc] initWithCapacity:0];
double ezx;
double ezy;
double ezz;
if ([P1 count] !=3){
    ezx = 0;
    ezy = 0;
    ezz = 0;

}else{
    ezx = ([[ex objectAtIndex:1] doubleValue]*[[ey objectAtIndex:2]doubleValue]) - ([[ex objectAtIndex:2]doubleValue]*[[ey objectAtIndex:1]doubleValue]);
    ezy = ([[ex objectAtIndex:2] doubleValue]*[[ey objectAtIndex:0]doubleValue]) - ([[ex objectAtIndex:0]doubleValue]*[[ey objectAtIndex:2]doubleValue]);
    ezz = ([[ex objectAtIndex:0] doubleValue]*[[ey objectAtIndex:1]doubleValue]) - ([[ex objectAtIndex:1]doubleValue]*[[ey objectAtIndex:0]doubleValue]);

}

[ez addObject:[NSNumber numberWithDouble:ezx]];
[ez addObject:[NSNumber numberWithDouble:ezy]];
[ez addObject:[NSNumber numberWithDouble:ezz]];


// d = numpy.linalg.norm(P2 - P1)
double d = sqrt(temp);

// j = dot(ey, P3 - P1)
double jval = 0;
for (int i = 0; i < [ey count]; i++) {
    double t1 = [[ey objectAtIndex:i] doubleValue];
    double t2 = [[p3p1 objectAtIndex:i] doubleValue];
    jval += (t1*t2);
}

// x = (pow(DistA,2) - pow(DistB,2) + pow(d,2))/(2*d)
double xval = (pow(DistA,2) - pow(DistB,2) + pow(d,2))/(2*d);

// y = ((pow(DistA,2) - pow(DistC,2) + pow(i,2) + pow(j,2))/(2*j)) - ((i/j)*x)
double yval = ((pow(DistA,2) - pow(DistC,2) + pow(ival,2) + pow(jval,2))/(2*jval)) - ((ival/jval)*xval);

// z = sqrt(pow(DistA,2) - pow(x,2) - pow(y,2))
// if 2-dimensional vector then z = 0
double zval;
if ([P1 count] !=3){
    zval = 0;
}else{
    zval = sqrt(pow(DistA,2) - pow(xval,2) - pow(yval,2));
}

// triPt = P1 + x*ex + y*ey + z*ez
NSMutableArray *triPt = [[NSMutableArray alloc] initWithCapacity:0];
for (int i = 0; i < [P1 count]; i++) {
    double t1 = [[P1 objectAtIndex:i] doubleValue];
    double t2 = [[ex objectAtIndex:i] doubleValue] * xval;
    double t3 = [[ey objectAtIndex:i] doubleValue] * yval;
    double t4 = [[ez objectAtIndex:i] doubleValue] * zval;
    double triptx = t1+t2+t3+t4;
    [triPt addObject:[NSNumber numberWithDouble:triptx]];
}

NSLog(@"ex %@",ex);
NSLog(@"i %f",ival);
NSLog(@"ey %@",ey);
NSLog(@"d %f",d);
NSLog(@"j %f",jval);
NSLog(@"x %f",xval);
NSLog(@"y %f",yval);
NSLog(@"y %f",yval);
NSLog(@"final result %@",triPt);

I've tested by drawing on cartesian diagram using the test case data above, and got the result that the unknown point is located at (8,4), then testing using the code above and got the result (7.999978,4.000021710625001).

Then I did a second test using data :

P1 = (2,0) r1 = 5.831
P2 = (8,0) r2 = 5.831
P3 = (8,10) r3 = 5.831

The manual result is (5,5), and the result using the code is (5,5). So, I believe the code is correct.

Polychrome

Nice solution but it works only with 3 points no more. That's not what the OP ask. – Azotobacter

This is 2D and is not accurate. You need to use 3D coordinates and find interception of 3 or more spheres to calculate where your location in a building is. Doing this way requires very complicated math tho. iBeacons works best if it is located above human heights. 3D consideration is very important. – Romero

Hi @Romero the equations above are formulated so that the centers of the spheres are on the z = 0 plane on the 3D, follow this article. But I agree it's not accurate, because in real case, many variable has to involved. And has to keep in mind the signal from beacon maybe interferred by the other signal, so the number you get from reading the beacon is maybe not clean. – Polychrome

how would you convert following steps. #convert back to lat/long from ECEF #convert to degrees lat = math.degrees(math.asin(triPt[2] / earthR)); lon = math.degrees(math.atan2(triPt[1],triPt[0])); They are for three points, not 2D vectors like your solution – Thanatopsis

@GemaMegantara, Nice solution for BLE using trilateration technique. Can you please clear me few things, the python implementation link gis.stackexchange.com/questions/66/… which you have given is for GPS right. Do we need to convert the ble coordinate to radians or else can we take the coordinate as such to identify ex, i, ey, ez, d and j – Mimir