CODEWITHSUNDEEP
objective-cibeacon
shadowarcher
shadowarcher

Reputation: 3563

iBeacon Trilateration - Vector Math

I'm having a bit of trouble with Trilateration using iBeacon's. I found a formula using vector math in this thread, and I implemented it in my project. However it seems to be giving me odd numbers.

It seems like it works most of the time, however I've noticed that sometimes it will give me extremely similar answers when using distances that are vastly different, which seems odd to me. 

CGPoint pointA = [[curBlufiPoints objectAtIndex:0] CGPointValue];
CGPoint pointB = [[curBlufiPoints objectAtIndex:1] CGPointValue];
CGPoint pointC = [[curBlufiPoints objectAtIndex:2] CGPointValue];

/*Point A = (40, 612)
  Point B = (379, 788)
  Point C = (0, 352)*/

float distanceA = [[distances objectAtIndex:0] floatValue];
float distanceB = [[distances objectAtIndex:1] floatValue];
float distanceC = [[distances objectAtIndex:2] floatValue];

/*distanceA = 13.535637
  distanceB = 46.931522
  distanceC = 51.585461
       ----OR----
  distanceA = 349.9057;
  distanceB = 352.84134;
  distanceC = 353.37515;*/


CGPoint P2MinusP1 = CGPointMake(pointB.x - pointA.x, pointB.y - pointA.y);
CGPoint P3MinusP1 = CGPointMake(pointC.x - pointA.x, pointC.y - pointA.y);
CGFloat magP2MinusP1 = sqrt(pow((P2MinusP1.x), 2) +  pow((P2MinusP1.y), 2));
CGPoint eX = CGPointMake(P2MinusP1.x / magP2MinusP1, P2MinusP1.y / magP2MinusP1);
CGFloat i = eX.x * P3MinusP1.x + eX.y * P3MinusP1.y;
CGPoint eXTimesI = CGPointMake(eX.x * i, eX.y * i);
CGPoint P3MinusP1MinusITimesEX = CGPointMake(P3MinusP1.x - eXTimesI.x, P3MinusP1.y - eXTimesI.y);
CGFloat magP3MinusP1MinusITimesEX = sqrt(pow(P3MinusP1MinusITimesEX.x, 2) + pow(P3MinusP1MinusITimesEX.y, 2));
CGPoint eY = CGPointMake(P3MinusP1MinusITimesEX.x / magP3MinusP1MinusITimesEX, P3MinusP1MinusITimesEX.y / magP3MinusP1MinusITimesEX);
CGFloat j = eY.x * P3MinusP1.x + eY.y * P3MinusP1.y;
CGFloat x = (pow(distanceA, 2) - pow(distanceB, 2) + pow(magP2MinusP1, 2)) / (2 * magP2MinusP1);
CGFloat y = (pow(distanceA, 2) - pow(distanceC, 2) + pow(i, 2) + pow(j, 2)) / (2 * j) - (i * x) / j;

CGPoint finalPoint = CGPointMake(pointA.x + x * eX.x + y * eY.x, pointA.y + x * eX.y + y * eY.y);

NSLog(@"%f %f %f %f", finalPoint.x, finalPoint.y);

//finalPoint.x = ~343
//finalPoint.y = ~437

As you can see from the values I've commented in the code above, when I am using the same points, but different distances I end up with the same result. It doesn't make sense to me how they can both have around the same answer. If the point (343, 437), (the answer I get for both sets of input) is 13.5 units away from point (40, 612), how can the same point also be 349.9 units away?

I'm not sure where my math has gone wrong but I think that something about calculating x and y is where the problems comes in. I've discovered that the lines "pow(distanceA, 2) - pow(distanceB, 2)" and "pow(distanceA, 2) - pow(distanceC, 2)" give me approximately the same answer for both sets of numbers, which is why x and y end up being the same regardless of which set of numbers I use.

I don't think that this should be the case, any help would be greatly appreciated, thank you!

Upvotes: 2

Views: 518

Answers (1)

Larme
Larme

Reputation: 26046

I tried your code with a sample (and bad looking code test). The testing code is here. To debug, I used the "3D" new feature of XCode 6, to see the points and circle that were out of the screen bounds, because I didn't want to do translation (recentering the plane, etc.)

The main issue (with your first sample test (@[@(13.535637), @(46.931522), @(51.585461)];) is that the circles aren't overlapping. There is NO intersection. So you can't guess the position. If you draw a circle with center pointA, and radius distanceA, and do it for point B and C, you'll need to find the intersection to geolocalize, which is clearly illustrated on the wiki-related article on the linked question.

I added a "check" method to see if they overlap correctly, with a "allowed mistake", because we're doing some maths with float and there could be a few rounding issue.
The check method consist on recalculating the distance between the finalPoint (coordinate found) and each point. The question is now if they differs (a lot?) from the original ones.

Screenshot with first set of distances

Screenshot with second set of distances

Upvotes: 2

Related Questions