Trilateration with approximated distances using latitude and longitude

Question

I can't quite figure this one out.

I am trying to approximate the location (latitude / longitude) of a beacon based on 3 distance measurements from 3 fixed locations. However the distance readings available may have an error of up to 1 km.

Similar questions regarding trilateration have been asked here (with precise measurements), here, here (distance measurement errors in Java, but not in lat/lon coordinates and no answers) as well as others. I also managed to dig up this paper dealing with imperfect measurement data, however it for one assumes a cartesian coordinate system and is also rather mathematical than close to a usable implementation.

So none of above links and answers are really applicable to the following problem:

• All available distance measurements are approximated in km (where data most frequently contains readings in-between 1 km and 100 km, in case this matters)
• Measurement errors of up to 1 km are possible.
• 3 distance measurements are performed based on 3 fixed (latitude / longitude known) positions.
• target approximation should also be a latitude / longitude combination.

So far I have adapted this Answer to C#, however I noticed that due to the measurement inaccuracies this algorithm does not work (as the algorithm assumes the 3 distance-circles to perfectly intersect with each other):

public static class Trilateration
{
    public static GeoLocation Compute(DistanceReading point1, DistanceReading point2, DistanceReading point3)
    {
        // not my code :P
        // assuming elevation = 0
        const double earthR = 6371d;

        //using authalic sphere
        //if using an ellipsoid this step is slightly different
        //Convert geodetic Lat/Long to ECEF xyz
        //   1. Convert Lat/Long to radians
        //   2d. Convert Lat/Long(radians) to ECEF
        double xA = earthR * (Math.Cos(Radians(point1.GeoLocation.Latitude)) * Math.Cos(Radians(point1.GeoLocation.Longitude)));
        double yA = earthR * (Math.Cos(Radians(point1.GeoLocation.Latitude)) * Math.Sin(Radians(point1.GeoLocation.Longitude)));
        double zA = earthR * Math.Sin(Radians(point1.GeoLocation.Latitude));

        double xB = earthR * (Math.Cos(Radians(point2.GeoLocation.Latitude)) * Math.Cos(Radians(point2.GeoLocation.Longitude)));
        double yB = earthR * (Math.Cos(Radians(point2.GeoLocation.Latitude)) * Math.Sin(Radians(point2.GeoLocation.Longitude)));
        double zB = earthR * (Math.Sin(Radians(point2.GeoLocation.Latitude)));

        double xC = earthR * (Math.Cos(Radians(point3.GeoLocation.Latitude)) * Math.Cos(Radians(point3.GeoLocation.Longitude)));
        double yC = earthR * (Math.Cos(Radians(point3.GeoLocation.Latitude)) * Math.Sin(Radians(point3.GeoLocation.Longitude)));
        double zC = earthR * Math.Sin(Radians(point3.GeoLocation.Latitude));

        // a 64 bit Vector3 implementation :)
        Vector3_64 P1 = new(xA, yA, zA);
        Vector3_64 P2 = new(xB, yB, zB);
        Vector3_64 P3 = new(xC, yC, zC);

        //from wikipedia
        //transform to get circle 1 at origin
        //ransform to get circle 2d on x axis
        Vector3_64 ex = (P2 - P1).Normalize();
        double i = Vector3_64.Dot(ex, P3 - P1);
        Vector3_64 ey = (P3 - P1 - i * ex).Normalize();
        Vector3_64 ez = Vector3_64.Cross(ex, ey);
        double d = (P2 - P1).Length;
        double j = Vector3_64.Dot(ey, P3 - P1);

        //from wikipedia
        //plug and chug using above values
        double x = (Math.Pow(point1.DistanceKm, 2d) - Math.Pow(point2.DistanceKm, 2d) + Math.Pow(d, 2d)) / (2d * d);
        double y = ((Math.Pow(point1.DistanceKm, 2d) - Math.Pow(point3.DistanceKm, 2d) + Math.Pow(i, 2d) + Math.Pow(j, 2d)) / (2d * j)) - ((i / j) * x);

        // only one case shown here
        double z = Math.Sqrt(Math.Pow(point1.DistanceKm, 2d) - Math.Pow(x, 2d) - Math.Pow(y, 2d));

        //triPt is a vector with ECEF x,y,z of trilateration point
        Vector3_64 triPt = P1 + x * ex + y * ey + z * ez;

        //convert back to lat/long from ECEF
        //convert to degrees
        double lat = Degrees(Math.Asin(triPt.Z / earthR));
        double lon = Degrees(Math.Atan2(triPt.Y, triPt.X));

        return new GeoLocation(lat, lon);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    private static double Radians(double degrees) =>
        degrees * Math.Tau / 360d;

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    private static double Degrees(double radians) =>
        radians * 360d / Math.Tau;
}

Above code most often than not does not work in my case, and instead only returns "Not a number" as it tries to take the square root of a negative number when calculating the final z value (due to measurement inaccuracies).

In my case measurements may return data like this (visualized with some random online tool):

where only 2 or even none of the distance circles intersect:

What I am looking for is the an algorithm returning the best possible approximation of the target location based on three distance measurements with a known maximum error of 1 km or further approaches I could take.

I have also thought of iterating over points on the circles to then determining the minimum average distance to all the points on the other circles but the 3-dimensional sphere geometry of the earth is giving me a headache. Also there's probably a way better and simpler approach to this which I just can't figure out right now.

As this is more of an algorithmic problem, rather than any language-specific thing, I appreciate any help in whatever programming language, pseudo code or natural language.

Answer 1

If you have access to a scientific computing library which provides non-linear optimization utilities, then you could try finding the point which minimizes the following:

(||x - p_1|| - r_1)^2 + (||x - p_2|| - r_2)^2 + (||x - p_3|| - r_3)^2 + (||x - p_earth|| - r_earth)^2

where p_i is the location (in Cartesian coordinates) of the ith location you measure from, r_i is the corresponding distance reading, p_earth is the location of the Earth, r_earth is the radius of the earth, and ||a|| denotes the norm/length of the vector a.

Each term in the expression is trying to minimize the residual radius error.

This can of course be modified to suit your needs - e.g. if constrained optimization is available, you could encode the requirement that the point be on the surface on the earth as a constraint rather than a term to optimize for. If spherical earth model isn't accurate enough, you could define an error from the Earth's surface, or just project your result onto the Earth if that is accurate enough.