Формула Trilateration (программирование)

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 & у = (0,0)
• PhoneB_x & у = (100,0)
• PhoneC_x & y = (50,100)
• phoneADistance = 0
• phoneBDistance = 100
• phoneCDistance = 111,803

он возвращает (-488,195, -366,147)

Контрольный пример # 2:

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

он возвращает (50, 37.5)

Answer 1

Это очень неряшливо, но это мой новый алгоритм.

Я просто взял таблицу 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; 
    }