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 behi

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.