BLE (iBeacons) Trilateration - objective-c

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

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.