三边测量公式(编程) [英] Trilateration Formula (Programming)
本文介绍了三边测量公式(编程)的处理方法,对大家解决问题具有一定的参考价值,需要的朋友们下面随着小编来一起学习吧!
问题描述
我目前正在尝试开发三边测量应用程序,以使用3部手机跟踪信标.我将在python中找到的代码转换为c#,但是我无法使其正常工作. 这是我的代码:
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;
}
当我运行此方法时
测试案例1:
- PhoneA_x& y =(0,0)
- PhoneB_x& y =(100,0)
- PhoneC_x& y =(50,100)
- phoneADistance = 0
- phoneBDistance = 100
- phoneCDistance = 111.803
它返回(-488.195,-366.147)
it returns (-488.195, -366.147)
测试案例2:
- PhoneA_x& y =(0,0)
- PhoneB_x& y =(100,0)
- PhoneC_x& y =(50,100)
- phoneADistance = 25
- phoneBDistance = 25
- phoneCDistance = 25
它返回(50,37.5)
it returns (50, 37.5)
推荐答案
这很草率,但这是我到目前为止的新算法.
It's very sloppy but this is my new algorithm so far.
我只是从Simon链接的网站上获取了Excel电子表格,并将其转换为C#代码.
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.
它仍在测试过程中,所以结果还不算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;
}
这篇关于三边测量公式(编程)的文章就介绍到这了,希望我们推荐的答案对大家有所帮助,也希望大家多多支持IT屋!
查看全文