Trilateration Formula (Programming)

Question

I'm currently trying to develop a trilateration application to track beacons using 3 phones. I converted code I found in python over to c# but I'm having trouble getting it to work. This is my code:

public double[] getPosition(double phoneADistance, double phoneBDistance, double phoneCDistance)
    {
        //meterToFeet is just a conversion method which takes the distance parameter and multiplies it by 3.28.
        double PhoneADist = meterToFeet(phoneADistance);
        double PhoneBDist = meterToFeet(phoneBDistance);
        double PhoneCDist = meterToFeet(phoneCDistance);

        //The phone's x and y coordinates are pre-set
        Vector<double> P1 = new DenseVector(new[] { PhoneA_x, PhoneA_y });
        Vector<double> P2 = new DenseVector(new[] { PhoneB_x, PhoneB_y });
        Vector<double> P3 = new DenseVector(new[] { PhoneC_x, PhoneC_y });

        var ex = (P2 - P1) / (P2 - P1).L2Norm();
        var i = ex.DotProduct(P3 - P1);
        var ey = (P3 - P1 - i * ex) / (P3 - P1 - i * ex).L2Norm();
        var d = (P2 - P1).L2Norm();
        var j = ey.DotProduct(P3 - P1);

        var x = (Math.Pow(PhoneADist, 2) - Math.Pow(PhoneBDist, 2) + Math.Pow(d, 2)) / (2 * d);
        var y = ((Math.Pow(PhoneADist, 2) - Math.Pow(PhoneCDist, 2) + Math.Pow(i, 2) + Math.Pow(j, 2)) / (2 * j)) - ((i / j) * x);


        double[] answer = new double[] { x, y };
        Console.Write(x + " " + y);
        return answer;
    }

When I run this method

Test case #1:

• PhoneA_x&y = (0,0)
• PhoneB_x&y = (100,0)
• PhoneC_x&y = (50,100)
• phoneADistance = 0
• phoneBDistance = 100
• phoneCDistance = 111.803

it returns (-488.195, -366.147)

Test Case #2:

• PhoneA_x&y = (0,0)
• PhoneB_x&y = (100,0)
• PhoneC_x&y = (50,100)
• phoneADistance = 25
• phoneBDistance = 25
• phoneCDistance = 25

it returns (50, 37.5)

Answer 1

It's very sloppy but this is my new algorithm so far.

I just took the excel spreadsheet from the site that Simon linked and converted into C# code.

There is still a lot of clean up to do.

It's still in testing process so I'm not 100 with the results, but from the testings I've done so far it seems pretty accurate.

    public double[] getPosition(double phoneADistance, double phoneBDistance, double phoneCDistance)
    {
        double[] answer = new double[] { 0, 0 };
        double PhoneADist = meterToFeet(phoneADistance);
        double PhoneBDist = meterToFeet(phoneBDistance);
        double PhoneCDist = meterToFeet(phoneCDistance);

        Vector<double> P1 = new DenseVector(new[] { PhoneA_x, PhoneA_y });
        Vector<double> P2 = new DenseVector(new[] { PhoneB_x, PhoneB_y });
        Vector<double> P3 = new DenseVector(new[] { PhoneC_x, PhoneC_y });

        //Translate values for the three points
        var B3 = PhoneA_x;
        var C3 = PhoneA_y;
        var D3 = phoneADistance;
        var B4 = PhoneB_x;
        var C4 = PhoneB_y;
        var D4 = phoneBDistance;
        var B5 = PhoneC_x;
        var C5 = PhoneC_y;
        var D5 = phoneCDistance;
        //Translate P1 to Origin
        var B8 = B3 - B3;
        var C8 = C3 - C3;
        var D8 = D3;
        var B9 = B4 - B3;
        var C9 = C4 - C3;
        var D9 = D4;
        var B10 = B5 - B3;
        var C10 = C5 - C3;
        var D10 = D5;
        //Find Calculation Values
        var B13 = Math.Atan2(C9, B9); ;
        var B14 = Math.Atan2(C10, B10);
        var B15 = Math.Sqrt(Math.Pow(B4 - B3, 2) + Math.Pow(C4 - C3, 2));
        var B16 = Math.Sqrt(Math.Pow(B5 - B3, 2) + Math.Pow(C5 - C3, 2));
        //Polar Coordinates for the Rotated System
        //var B20 = 0;
        //var C20 = 0;
        var D20 = D3;
        var B21 = B15;
        //var C21 = 0;
        var D21 = D4;
        var B22 = B16;
        var C22 = B14 - B13;
        var D22 = D5;
        //Rectangular Coordinates for the Rotated System
        //var B26 = 0;
        //var C26 = 0;
        var D26 = D3;
        var B27 = B21;
        //var C27 = 0;
        var D27 = D4;
        var B28 = B22 * Math.Cos(C22);
        var C28 = B22 * Math.Sin(C22);
        var D28 = D5;
        //Coordinates of Roated Solution
        var B31 = (Math.Pow(D3, 2) - Math.Pow(D4, 2) + Math.Pow(B27, 2)) / (B27 * 2);
        var B32 = Math.Sqrt(Math.Pow(D3, 2) - Math.Pow(B31, 2));
        var D32 = -B32;
        //Convert to Polar
        var B35 = Math.Sqrt(Math.Pow(B31, 2) + Math.Pow(B32, 2));
        var B36 = Math.Atan2(B32, B31);
        var D36 = Math.Atan2(D32, B31);
        //Unrotate
        var B39 = B35;
        var B40 = B36 + B13;
        var D40 = D36 + B13;
        //Rectangular Coordinates
        var B43 = B39 * Math.Cos(B40);
        var D43 = B39 * Math.Cos(D40);
        var B44 = B39 * Math.Sin(B40);
        var D44 = B39 * Math.Sin(D40);
        //Untranslate
        var B47 = B43 + B3;
        var D47 = D43 + B3;
        var B48 = B44 + C3;
        var D48 = D44 + C3;
        var x = B47;
        var y = B48;
        //Return Answer
        if (!Double.IsNaN(x) || !Double.IsNaN(y))
        {
            answer = new double[] { x, y };
            Console.Write(x + " " + y);
        }
        return answer;
    }