Looking for Trilateration algorithm

Unity Engine
1

Hi all,

I am looking a trilateration algorithm that can use 3 position (Vector3) with its 3 radius or any other algorithm for calculate centroid of 3 position (Vector3) with its 3 radius.

For the moment i use this algorithm:

    public static Vector3 GetTrilaterationPosition(Vector3 beaconA, Vector3 beaconB, Vector3 beaconC, float radiusA, float radiusB, float radiusC)
    {
        //PARAMETERS

        float bAlat = beaconA.x;
        float bAlong = beaconA.z;
        float bBlat = beaconB.x;
        float bBlong = beaconB.z;
        float bClat = beaconC.x;
        float bClong = beaconC.z;

        //TEST 1

        float W, Z, foundBeaconLat, foundBeaconLong, foundBeaconLongFilter;
        W = radiusA * radiusA - radiusB * radiusB - bAlat * bAlat - bAlong * bAlong + bBlat * bBlat + bBlong * bBlong;
        Z = radiusB * radiusB - radiusC * radiusC - 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;

        var foundLocation = new Vector3();
        foundLocation.x = (foundBeaconLat);
        foundLocation.z = (foundBeaconLong);

        return foundLocation;


    }

But it’s not much precise and sometimes the calculation throw bad positions, i am not very good with mathematics =(

Any suggestion or advice?

Sorry for my english.

2

I can’t say I fully understand what that algorithm is doing, but it seems to me that there’s already a problem in that it discards the y axis on the vectors, but doesn’t normalize the resulting 2D vector.

Unless the beacons are placed flush against the surface, and there’s no displacement in Y going on, that’s one potential problem area already.

In 3D space, trilateration can give you 2 valid solutions. Imagine each beacon is surrounded by a sphere of the given radius, there are two points where the three spheres will intersect. One above the beacons and another below them. (plus a singularity case if the test position is exactly at 0 along Y)

If you’re reducing the problem to 2D space, but the input radii are still from the 3D distances to each beacon (and Y is non-zero), your input parameters will never fit the constraints, and you will probably see inaccurate, jittery results.

Not sure it will help, but it’s somewhere to start.

Cheers