c# - 三边测量公式(编程)

Question

我目前正在尝试开发一个三边测量应用程序来使用 3 部手机跟踪信标。我将我在 python 中找到的代码转换为 c#，但我无法让它工作。 这是我的代码:

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;
    }

当我运行这个方法时

测试用例#1:

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

它返回 (-488.195, -366.147)

测试用例#2:

• PhoneA_x&y = (0,0)
• 电话B_x&y = (100,0)
• PhoneC_x&y = (50,100)
• phoneADistance = 25
• phoneBDistance = 25
• 电话CDistance = 25

它返回 (50, 37.5)

Answer 1

这很草率，但这是我到目前为止的新算法。

我刚刚从 Simon 链接的站点获取了 excel 电子表格并将其转换为 C# 代码。

还有很多清理工作要做。

它仍在测试过程中，所以我对结果不是 100，但从我到目前为止所做的测试来看，它似乎相当准确。

    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;
    }