Triangulate example for iBeacons

I am looking into the possibility to use multiple iBeacons to do a 'rough' indoor position location. The application is a kind of 'museum' setting, and it would be easier to be able to form a grid with locations for the different objects then individual beacons (although that might not be impossible too).

Are there examples, experiences, with using multiple beacons to triangulate into some kind of location, or some logic to help me on the way to write it myself?

I have been making some experiments to get a precise position using three beacons.

Results of trilateration

Unluckily, the results were very disappointing in terms of quality. There were mainly two issues:

1. In non-controlled environments, where you can find metals, and other objects that affect the signal, the received signal strength of the beacons changes so often that it seems impossible to get error range below 5 meters.
2. Depending on the way that the user is handling the receiver device, the readings can change a lot as well. If the user puts his/her hand over the bluetooth antenna, then the algorithm will have low signals as input, and thus the beacons will supposed to be very far from the device. See this image to see the precise location of the Bluetooth antenna.

Possible solutions

After talking with an Apple engineer who actively discouraged me to go down this way, the option I feel more inclined to use right now is brute force. Try to set up a beacon every X meters (X being the maximum error tolerated in the system) so we can track on this beacons grid the position of a given device by calculating which beacon on the grid is the closest to the device and assuming that the device is on the same position.

Trilateration algorithm

However, for the sake of completeness, I share below the core function of the trilateration algorithm. It's based on the paragraph 3 ("Three distances known") of this article.

- (CGPoint)getCoordinateWithBeaconA:(CGPoint)a beaconB:(CGPoint)b beaconC:(CGPoint)c distanceA:(CGFloat)dA distanceB:(CGFloat)dB distanceC:(CGFloat)dC {
    CGFloat W, Z, x, y, y2;
    W = dA*dA - dB*dB - a.x*a.x - a.y*a.y + b.x*b.x + b.y*b.y;
    Z = dB*dB - dC*dC - b.x*b.x - b.y*b.y + c.x*c.x + c.y*c.y;
    
    x = (W*(c.y-b.y) - Z*(b.y-a.y)) / (2 * ((b.x-a.x)*(c.y-b.y) - (c.x-b.x)*(b.y-a.y)));
    y = (W - 2*x*(b.x-a.x)) / (2*(b.y-a.y));
    //y2 is a second measure of y to mitigate errors
    y2 = (Z - 2*x*(c.x-b.x)) / (2*(c.y-b.y));
    
    y = (y + y2) / 2;
    return CGPointMake(x, y);
}

Here is an open source java library that will perform the trilateration/multilateration: https://github.com/lemmingapex/Trilateration

It uses a popular nonlinear least squares optimizer, the Levenberg-Marquardt algorithm, from Apache Commons Math.

double[][] positions = new double[][] { { 5.0, -6.0 }, { 13.0, -15.0 }, { 21.0, -3.0 }, { 12.42, -21.2 } };
double[] distances = new double[] { 8.06, 13.97, 23.32, 15.31 };

NonLinearLeastSquaresSolver solver = new NonLinearLeastSquaresSolver(new TrilaterationFunction(positions, distances), new LevenbergMarquardtOptimizer());
Optimum optimum = solver.solve();

// the answer
double[] calculatedPosition = optimum.getPoint().toArray();

// error and geometry information
RealVector standardDeviation = optimum.getSigma(0);
RealMatrix covarianceMatrix = optimum.getCovariances(0);

Most scholarly examples, like the one on wikipedia, deal with exactly three circles and assume perfectly accurate information. These circumstances allow for much simpler problem formulations with exact answers, and are usually not satisfactory for practical situations.

The problem in R² or R³ euclidean space with distances that contain measurement error, an area (ellipse) or volume (ellipsoid) of interest is usually obtained instead of a point. If a point estimate is desired instead of a region, the area centroid or volume centroid should be used. R² space requires at least 3 non-degenerate points and distances to obtain a unique region; and similarly R³ space requires at least 4 non-degenerate points and distances to obtain a unique region.

I looked into this. The term you want it trilateration. (In triangulation you have angles from 3 known points. In trilateration you have distance from 3 known points) If you Google it you should find several articles including one on Wiki. It involves solving a set of 3 simultaneous equations. The documents I saw were for 3D trilateration - 2D is easier because you can just drop the Z term.

What I found was abstract math. I haven't taken the time yet to map the general algorithm into specific code, but I plan on tackling it at some point.

Note that the results you get will be VERY crude, especially in anything but an empty room. The signals are weak enough that a person, a statue, or anything that blocks line of sight will increase your distance readings pretty significantly. You might even have places in a building where constructive interference (mostly from the walls) makes some places read as much closer than they actually are.

Accurate indoor positioning with iBeacon will be challenging for the following reasons:

1. As pointed in earlier comments, iBeacon signal tend to fluctuate a lot. The reason include multipath effect, the dynamic object obstructions between the phone and iBeacon when the person is moving, other 2.4GHz interferences, and more. So ideally you don't want to trust 1 single packet's data and instead do some averaging for several packets from the same beacon. That would require the phone/beacon distance doesn't change too much between those several packets. For general BLE packets (like beacons from StickNFind) can easily be set to 10Hz beaconing rate. However for iBeacon, that'll be hard, because
2. iBeacon's beaconing frequency probably cannot be higher than 1Hz. I will be glad if anyone can point to source that says otherwise, but all information I've seen so far confirms this assertion. That actually make sense since most iBeacons will be battery powered and high frequency significantly impact the battery life. Considering people's average walking speed is 5.3km (~1.5m/s), so even if you just use a modest 3 beacon packets to do the averaging, you will be hard to get ~5m accuracy.

On the other hand, if you could increase iBeacon frequency to larger than 10Hz (which I doubt is possible), then it's possible to have 5m or higher accuracy using suitable processing method. Firstly trivial solutions based on the Inverse-Square Law, like trilateration, is often not performing well because in practice the distance/RSSI relationship for different beacons are often way off from the Inverse-Sqare Law for the reason 1 above. But as long as the RSSI is relatively stable for a certain beacon in any certain location (which usually is the case), you can use an approach called fingerprinting to achieve higher accuracy. A common method used for fingerprinting is kNN (k-Nearest Neighbor).

Update 2014-04-24

Some iBeacons can broadcast more than 1Hz, like Estimote use 5Hz as default. However, according to this link: "This is Apple restriction. IOS returns beacons update every second, no matter how frequently device is advertising.". There is another comment there (likely from the Estimote vendor) saying "Our beacons can broadcast much faster and it may improve results and measurement". So whether higher iBeacon frequency is beneficial is not clear.

For those who need @Javier Chávarri trilateration function for Android devices (for saving some time):

public static Location getLocationWithTrilateration(Location beaconA, Location beaconB, Location beaconC, double distanceA, double distanceB, double distanceC){

    double bAlat = beaconA.getLatitude();
    double bAlong = beaconA.getLongitude();
    double bBlat = beaconB.getLatitude();
    double bBlong = beaconB.getLongitude();
    double bClat = beaconC.getLatitude();
    double bClong = beaconC.getLongitude();

    double W, Z, foundBeaconLat, foundBeaconLong, foundBeaconLongFilter;
    W = distanceA * distanceA - distanceB * distanceB - bAlat * bAlat - bAlong * bAlong + bBlat * bBlat + bBlong * bBlong;
    Z = distanceB * distanceB - distanceC * distanceC - bBlat * bBlat - bBlong * bBlong + bClat * bClat + bClong * bClong;

    foundBeaconLat = (W * (bClong - bBlong) - Z * (bBlong - bAlong)) / (2 * ((bBlat - bAlat) * (bClong - bBlong) - (bClat - bBlat) * (bBlong - bAlong)));
    foundBeaconLong = (W - 2 * foundBeaconLat * (bBlat - bAlat)) / (2 * (bBlong - bAlong));
    //`foundBeaconLongFilter` is a second measure of `foundBeaconLong` to mitigate errors
    foundBeaconLongFilter = (Z - 2 * foundBeaconLat * (bClat - bBlat)) / (2 * (bClong - bBlong));

    foundBeaconLong = (foundBeaconLong + foundBeaconLongFilter) / 2;

    Location foundLocation = new Location("Location");
        foundLocation.setLatitude(foundBeaconLat);
        foundLocation.setLongitude(foundBeaconLong);

    return foundLocation;
}

If you're anything like me and don't like maths you might want to do a quick search for "indoor positioning sdk". There's lots of companies offering indoor positioning as a service.

Shameless plug: I work for indoo.rs and can recommend this service. It also includes routing and such on top of "just" indoor positioning.

My Architect/Manager, who wrote the following algorithm,

public static Location getLocationWithCenterOfGravity(Location beaconA, Location beaconB, Location beaconC, double distanceA, double distanceB, double distanceC) {

    //Every meter there are approx 4.5 points
    double METERS_IN_COORDINATE_UNITS_RATIO = 4.5;

    //http://stackoverflow.com/a/524770/663941
    //Find Center of Gravity
    double cogX = (beaconA.getLatitude() + beaconB.getLatitude() + beaconC.getLatitude()) / 3;
    double cogY = (beaconA.getLongitude() + beaconB.getLongitude() + beaconC.getLongitude()) / 3;
    Location cog = new Location("Cog");
    cog.setLatitude(cogX);
    cog.setLongitude(cogY);


    //Nearest Beacon
    Location nearestBeacon;
    double shortestDistanceInMeters;
    if (distanceA < distanceB && distanceA < distanceC) {
        nearestBeacon = beaconA;
        shortestDistanceInMeters = distanceA;
    } else if (distanceB < distanceC) {
        nearestBeacon = beaconB;
        shortestDistanceInMeters = distanceB;
    } else {
        nearestBeacon = beaconC;
        shortestDistanceInMeters = distanceC;
    }

    //http://www.mathplanet.com/education/algebra-2/conic-sections/distance-between-two-points-and-the-midpoint
    //Distance between nearest beacon and COG
    double distanceToCog = Math.sqrt(Math.pow(cog.getLatitude() - nearestBeacon.getLatitude(),2)
            + Math.pow(cog.getLongitude() - nearestBeacon.getLongitude(),2));

    //Convert shortest distance in meters into coordinates units.
    double shortestDistanceInCoordinationUnits = shortestDistanceInMeters * METERS_IN_COORDINATE_UNITS_RATIO;

    //http://math.stackexchange.com/questions/46527/coordinates-of-point-on-a-line-defined-by-two-other-points-with-a-known-distance?rq=1
    //On the line between Nearest Beacon and COG find shortestDistance point apart from Nearest Beacon

    double t = shortestDistanceInCoordinationUnits/distanceToCog;

    Location pointsDiff = new Location("PointsDiff");
    pointsDiff.setLatitude(cog.getLatitude() - nearestBeacon.getLatitude());
    pointsDiff.setLongitude(cog.getLongitude() - nearestBeacon.getLongitude());

    Location tTimesDiff = new Location("tTimesDiff");
    tTimesDiff.setLatitude( pointsDiff.getLatitude() * t );
    tTimesDiff.setLongitude(pointsDiff.getLongitude() * t);

    //Add t times diff with nearestBeacon to find coordinates at a distance from nearest beacon in line to COG.

    Location userLocation = new Location("UserLocation");
    userLocation.setLatitude(nearestBeacon.getLatitude() + tTimesDiff.getLatitude());
    userLocation.setLongitude(nearestBeacon.getLongitude() + tTimesDiff.getLongitude());

    return userLocation;
}

1. Calculate the centre of gravity for a triangle (3 beacons)
2. calculate the shortest distance / nearest beacon
3. Calculate the distance between the beacon and the centre of gravity
4. Convert the shortest distance to co-ordinate units which is just a constant, he used to predict accuracy. You can test with varing the constant
5. calculate the distance delta
6. add the delta with the nearest beacon x,y.

After testing it, I found it accurate to 5 meters.

Please comment me your testing, if we can refine it.

I've implemented a very simple Fingerprint algorithm for android 4.4, tested in a relative 'bad' environment:

• nearly 10 wifi AP nearby.
• several other Bluetooth signals nearby.

the accurate seems in 5-8 meters and depends on how I placed that 3 Ibeacon broadcaster. The algorithm is quite simple and I think you can implemented one by yourself, the steps are:

1. load the indoor map.
2. sampling with the map for all the pending positioning point.
3. record all the sampling data, the data should include: map coordinate, position signals and their RSSI.

so when you start positioning, it's just a reverse of proceeding steps.

We are also trying to find the best way to precisely locate someone into a room using iBeacons. The thing is that the beacon signal power is not constant, and it is affected by other 2.4 Ghz signals, metal objects etc, so to achieve maximum precision it is necessary to calibrate each beacon individually, and once it has been set in the desired position. (and make some field test to see signal fluctuations when other Bluetooth devices are present). We have also some iBeacons from Estimote (the same of the Konrad Dzwinel's video), and they have already developed some tech demo of what can be done with the iBeacons. Within their App it is possible to see a Radar in which iBeacons are shown. Sometimes is pretty accurate, but sometimes it is not, (and seems phone movement is not being considered to calculate positions). Check the Demo in the video we made here: http://goo.gl/98hiza

Although in theory 3 iBeacons should be enough to achieve a good precision, maybe in real world situations more beacons are needed to ensure the precision you are looking for.

The thing that really helped me was this project on Code.Google.com: https://code.google.com/p/wsnlocalizationscala/ it contains lots of code, several trilateration algorithms, all written in C#. It's a big library, but not really meant to be used "out-of-the-box".

Please check the reference https://proximi.io/accurate-indoor-positioning-bluetooth-beacons/

Proximi SDK will take care of the triangulation. This SDK provides libraries for handling all the logic for beacon positioning, triangulation and filtering automatically in the background. In addition to beacons, you can combine IndoorAtlas, Wi-Fi, GPS and cellular positioning.

I found Vishnu Prahbu's solution very useful. I ported it to c#, if anybody need it.

public static PointF GetLocationWithCenterOfGravity(PointF a, PointF b, PointF c, float dA, float dB, float dC)
    {
        //http://stackoverflow.com/questions/20332856/triangulate-example-for-ibeacons
        var METERS_IN_COORDINATE_UNITS_RATIO = 1.0f;

        //http://stackoverflow.com/a/524770/663941
        //Find Center of Gravity
        var cogX = (a.X + b.X + c.X) / 3;
        var cogY = (a.Y + b.Y + c.Y) / 3;
        var cog = new PointF(cogX,cogY);
        
        //Nearest Beacon
        PointF nearestBeacon;
        float shortestDistanceInMeters;
        if (dA < dB && dA < dC)
        {
            nearestBeacon = a;
            shortestDistanceInMeters = dA;
        }
        else if (dB < dC)
        {
            nearestBeacon = b;
            shortestDistanceInMeters = dB;
        }
        else
        {
            nearestBeacon = c;
            shortestDistanceInMeters = dC;
        }

        //http://www.mathplanet.com/education/algebra-2/conic-sections/distance-between-two-points-and-the-midpoint
        //Distance between nearest beacon and COG
        var distanceToCog =  (float)(Math.Sqrt(Math.Pow(cog.X - nearestBeacon.X, 2)
                + Math.Pow(cog.Y - nearestBeacon.Y, 2)));

        //Convert shortest distance in meters into coordinates units.
        var shortestDistanceInCoordinationUnits = shortestDistanceInMeters * METERS_IN_COORDINATE_UNITS_RATIO;

        //http://math.stackexchange.com/questions/46527/coordinates-of-point-on-a-line-defined-by-two-other-points-with-a-known-distance?rq=1
        //On the line between Nearest Beacon and COG find shortestDistance point apart from Nearest Beacon
        var t = shortestDistanceInCoordinationUnits / distanceToCog;
        var pointsDiff = new PointF(cog.X - nearestBeacon.X, cog.Y - nearestBeacon.Y);
        var tTimesDiff = new PointF(pointsDiff.X * t, pointsDiff.Y * t);
       
        //Add t times diff with nearestBeacon to find coordinates at a distance from nearest beacon in line to COG.
        var userLocation = new PointF(nearestBeacon.X + tTimesDiff.X, nearestBeacon.Y + tTimesDiff.Y);
        
        return userLocation;
    }

Alternative Equation

- (CGPoint)getCoordinateWithBeaconA:(CGPoint)a beaconB:(CGPoint)b beaconC:(CGPoint)c distanceA:(CGFloat)dA distanceB:(CGFloat)dB distanceC:(CGFloat)dC {


CGFloat x, y;
x = ( ( (pow(dA,2)-pow(dB,2)) + (pow(c.x,2)-pow(a.x,2)) + (pow(b.y,2)-pow(a.y,2)) ) * (2*c.y-2*b.y) - ( (pow(dB,2)-pow(dC,2)) + (pow(c.x,2)-pow(c.x,2)) + (pow(c.y,2)-pow(b.y,2)) ) *(2*b.y-2*a.y) ) / ( (2*b.x-2*c.x)*(2*b.y-2*a.y)-(2*a.x-2*b.x)*(2*c.y-2*b.y) );
                                         
y = ( (pow(dA,2)-pow(dB,2)) + (pow(c.x,2)-pow(a.x,2)) + (pow(b.y,2)-pow(a.y,2)) + x*(2*a.x-2*b.x)) / (2*b.y-2*a.y);
                                         
                                         
                                         
return CGPointMake(x, y);
}