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:
it returns (-488.195, -366.147)
Test Case #2:
it returns (50, 37.5)
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;
}