2D Trilateration using linear least squares in C# - c#

I am doing 2D trilateration using the MathNet for the matrices and vectors. This i my code:
public static double[] trilaterate2DLinear(double[] pA, double[] pB, double[] pC, double rA, double rB, double rC) {
//Convert doubles to vectors for processing
Vector<double> vA = Vector<double>.Build.Dense(pA);
Vector<double> vB = Vector<double>.Build.Dense(pB);
Vector<double> vC = Vector<double>.Build.Dense(pC);
//Declare elements of b vector
//bBA = 1/2 * (rA^2 - rB^2 + dBA^2)
double[] b = {0, 0};
b[0] = 0.5 * (Math.Pow(rA, 2) - Math.Pow(rB, 2) + Math.Pow(getDistance(pB, pA), 2));
b[1] = 0.5 * (Math.Pow(rA, 2) - Math.Pow(rC, 2) + Math.Pow(getDistance(pC, pA), 2));
//Convert b array to vector form
Vector<double> vb = Vector<double>.Build.Dense(b);
//Build A array
//A = {x2 -x1, y2 - y1}
// {x3 - x1, y3 - y1}
double[,] A = { { pB[0] - pA[0], pB[1] - pA[1] }, { pC[0] - pA[0], pC[1] - pA[1] } };
//Convert A to Matrix form
Matrix<double> mA = Matrix<double>.Build.DenseOfArray(A);
//Declare Transpose of A matrix;
Matrix<double> mAT = mA.Transpose();
//Declare solution vector x to 0
Vector<double> x = Vector<double>.Build.Dense(2);
//Check if A*AT is non-singular (non 0 determinant)
if (mA.Multiply(mAT).Determinant() == 0)
{
//x = ((AT * A)^-1)*AT*b
x = (((mA.Multiply(mAT)).Inverse()).Multiply(mAT)).Multiply(vb);
}
else
{
//TODO case for A*AT to be singular
x = (((mA.Multiply(mAT)).Inverse()).Multiply(mAT)).Multiply(vb);
}
//final position is x + vA
//return as double so as not
return (x.Add(vA)).ToArray();
}
//Gets the Euclidean distance between two points
private static double getDistance(double[] p1, double[] p2)
{
//d^2 = (p1[0] - p2[0])^2 + (p1[1] - p2[1]);
double distSquared = Math.Pow((p1[0] - p2[0]),2) + Math.Pow((p1[1] - p2[1]),2);
return Math.Sqrt(distSquared);
}
pA, pB & pC are the coordinates of the the Beacons and rA, rB & rC are the distances from the each beacon to the user. Is there anything obvious I am doing wrong? Maybe the order of Matrix multiplications need to change but I am not familiar enough with the Linear Least Squares to be able to track the Matrices and tell.

Solved. The if statement condition and calculations inside the if statement where wrong.
Correction:
public static double[] trilaterate2DLinear(double[] pA, double[] pB, double[] pC, double rA, double rB, double rC) {
//Convert doubles to vectors for processing
Vector<double> vA = Vector<double>.Build.Dense(pA);
Vector<double> vB = Vector<double>.Build.Dense(pB);
Vector<double> vC = Vector<double>.Build.Dense(pC);
//Declare elements of b vector
//bBA = 1/2 * (rA^2 - rB^2 + dBA^2)
double[] b = {0, 0};
b[0] = 0.5 * (Math.Pow(rA, 2) - Math.Pow(rB, 2) + Math.Pow(getDistance(pB, pA), 2));
b[1] = 0.5 * (Math.Pow(rA, 2) - Math.Pow(rC, 2) + Math.Pow(getDistance(pC, pA), 2));
//Convert b array to vector form
Vector<double> vb = Vector<double>.Build.Dense(b);
//Build A array
//A = {x2 -x1, y2 - y1}
// {x3 - x1, y3 - y1}
double[,] A = { { pB[0] - pA[0], pB[1] - pA[1] }, { pC[0] - pA[0], pC[1] - pA[1] } };
//Convert A to Matrix form
Matrix<double> mA = Matrix<double>.Build.DenseOfArray(A);
//Declare Transpose of A matrix;
Matrix<double> mAT = mA.Transpose();
//Declare solution vector x to 0
Vector<double> x = Vector<double>.Build.Dense(2);
//Check if A*AT is non-singular (non 0 determinant)
double det = mA.Multiply(mAT).Determinant();
if (mA.Multiply(mAT).Determinant() > 0.1)
{
//x = ((AT * A)^-1)*AT*b
// x = (((mA.Multiply(mAT)).Inverse()).Multiply(mAT)).Multiply(vb);
x = (mA.Transpose() * mA).Inverse() * (mA.Transpose() * vb);
}
else
{
//TODO case for A*AT to be singular
x = (((mA.Multiply(mAT)).Inverse()).Multiply(mAT)).Multiply(vb);
}
//final position is x + vA
//return as double so as not
return (x.Add(vA)).ToArray();
}

You are not calculating the B vector correctly.
It should be:
//dBA = 0.5 * (rA^2 - rB^2 - length_vA^2 + length_vB^2)
b[0] = 0.5 * (Math.Pow(rA, 2) - Math.Pow(rB, 2) - Math.Pow(getDistance(pA,{0,0}), 2) + Math.Pow(getDistance(pB,{0,0}), 2));
b[1] = 0.5 * (Math.Pow(rA, 2) - Math.Pow(rC, 2) - Math.Pow(getDistance(pA,{0,0}), 2) + Math.Pow(getDistance(pC,{0,0}), 2));

Related

Solving equation to find center point of circle from 3 points

I'm looking for a high precision solution to find the center point of a circle from 3 data points on a canvas (x,y). I found this example in the attached screenshot above, now I'm using the Math.NET package to solve the equation and I'm comparing the results against this online tool: https://planetcalc.com/8116/.
However, when I calculate the radius its completely off and often a negative number???
using MathNet.Numerics.LinearAlgebra.Double.Solvers;
using MathNet.Numerics.LinearAlgebra.Double;
using System;
namespace ConsoleAppTestBed
{
class Program
{
static void Main(string[] args)
{
var dataPoints = new double[,]
{
{ 5, 80 },
{ 20, 100 },
{ 40, 140 }
};
var fitter = new CircleFitter();
var result = fitter.Fit(dataPoints);
var x = -result[0];
var y = -result[1];
var c = result[2];
Console.WriteLine("Center Point:");
Console.WriteLine(x);
Console.WriteLine(y);
Console.WriteLine(c);
//// (x^2 + y^2 - c^2)
var radius = Math.Pow(x, 2) + Math.Pow(y, 2) - Math.Pow(c, 2);
//// sqrt((x^2 + y^2 - c^2))
radius = Math.Sqrt(radius);
Console.WriteLine("Radius:");
Console.WriteLine(radius);
Console.ReadLine();
}
public class CircleFitter
{
public double[] Fit(double[,] v)
{
var xy1 = new double[] { v[0,0], v[0,1] };
var xy2= new double[] { v[1, 0], v[1, 1] };
var xy3 = new double[] { v[2, 0], v[2, 1] };
// Create Left Side Matrix of Equation
var a = CreateLeftSide_(xy1);
var b = CreateLeftSide_(xy2);
var c = CreateLeftSide_(xy3);
var matrixA = DenseMatrix.OfArray(new[,]
{
{ a[0], a[1], a[2] },
{ b[0], b[1], b[2] },
{ c[0], c[1], c[2] }
});
// Create Right Side Vector of Equation
var d = CreateRightSide_(xy1);
var e = CreateRightSide_(xy2);
var f = CreateRightSide_(xy3);
double[] vector = { d, e, f };
var vectorB = Vector<double>.Build.Dense(vector);
// Solve Equation
var r = matrixA.Solve(vectorB);
var result = r.ToArray();
return result;
}
//2x, 2y, 1
public double[] CreateLeftSide_(double[] d)
{
return new double[] { (2 * d[0]), (2 * d[1]) , 1};
}
// -(x^2 + y^2)
public double CreateRightSide_(double[] d)
{
return -(Math.Pow(d[0], 2) + Math.Pow(d[1], 2));
}
}
}
}
Any ideas?
Thanks in advance.
The solution to your problem is here: The NumberDecimalDigits property
Code:
using System;
using System.Globalization;
namespace ConsoleApp1
{
class Program
{
static void Main()
{
double x1 = 1, y1 = 1;
double x2 = 2, y2 = 4;
double x3 = 5, y3 = -3;
findCircle(x1, y1, x2, y2, x3, y3);
Console.ReadKey();
}
static void findCircle(double x1, double y1,
double x2, double y2,
double x3, double y3)
{
NumberFormatInfo setPrecision = new NumberFormatInfo();
setPrecision.NumberDecimalDigits = 3; // 3 digits after the double point
double x12 = x1 - x2;
double x13 = x1 - x3;
double y12 = y1 - y2;
double y13 = y1 - y3;
double y31 = y3 - y1;
double y21 = y2 - y1;
double x31 = x3 - x1;
double x21 = x2 - x1;
double sx13 = (double)(Math.Pow(x1, 2) -
Math.Pow(x3, 2));
double sy13 = (double)(Math.Pow(y1, 2) -
Math.Pow(y3, 2));
double sx21 = (double)(Math.Pow(x2, 2) -
Math.Pow(x1, 2));
double sy21 = (double)(Math.Pow(y2, 2) -
Math.Pow(y1, 2));
double f = ((sx13) * (x12)
+ (sy13) * (x12)
+ (sx21) * (x13)
+ (sy21) * (x13))
/ (2 * ((y31) * (x12) - (y21) * (x13)));
double g = ((sx13) * (y12)
+ (sy13) * (y12)
+ (sx21) * (y13)
+ (sy21) * (y13))
/ (2 * ((x31) * (y12) - (x21) * (y13)));
double c = -(double)Math.Pow(x1, 2) - (double)Math.Pow(y1, 2) -
2 * g * x1 - 2 * f * y1;
double h = -g;
double k = -f;
double sqr_of_r = h * h + k * k - c;
// r is the radius
double r = Math.Round(Math.Sqrt(sqr_of_r), 5);
Console.WriteLine("Center of a circle: x = " + h.ToString("N", setPrecision) +
", y = " + k.ToString("N", setPrecision));
Console.WriteLine("Radius: " + r.ToString("N", setPrecision));
}
}
}
I have just converted William Li's answer to Swift 5 for
those who like to cmd+C and cmd+V like me :)
func calculateCircle(){
let x1:Float = 0
let y1:Float = 0
let x2:Float = 0.5
let y2:Float = 0.5
let x3:Float = 1
let y3:Float = 0
let x12 = x1 - x2
let x13 = x1 - x3
let y12 = y1 - y2
let y13 = y1 - y3
let y31 = y3 - y1
let y21 = y2 - y1
let x31 = x3 - x1
let x21 = x2 - x1
let sx13 = pow(x1, 2) - pow(x3, 2)
let sy13 = pow(y1, 2) - pow(y3, 2)
let sx21 = pow(x2, 2) - pow(x1, 2)
let sy21 = pow(y2, 2) - pow(y1, 2)
let f = ((sx13) * (x12)
+ (sy13) * (x12)
+ (sx21) * (x13)
+ (sy21) * (x13))
/ (2 * ((y31) * (x12) - (y21) * (x13)))
let g = ((sx13) * (y12)
+ (sy13) * (y12)
+ (sx21) * (y13)
+ (sy21) * (y13))
/ (2 * ((x31) * (y12) - (x21) * (y13)))
let c = -pow(x1, 2) - pow(y1, 2) - 2 * g * x1 - 2 * f * y1
let h = -g
let k = -f
let r = sqrt(h * h + k * k - c)
print("center x = \(h)")
print("center y = \(k)")
print("r = \(r)")
}
Updated Answer
The equation for radius is incorrect; it should be (not c squared):
which is why you get incorrect values for the radius.
The original answer was incorrect, but it is still 'interesting'.
(Incorrect) Original Answer
The cause of the problematic calculation is not solely due to the precision of the numbers, but more because problem is ill-conditioned. If you look at the three points and where they are located on the circle, you'll see that they are bunched on a small segment of the circumference. When the points are so close to each other, it is a tough ask to find the circle's centre and radius accurately.
So the intermediate calculations, which will have small rounding errors, result in hugely exaggerated errors.
You can see the ill-conditioned nature of the problem by adding the ConditionNumber() method.
// Solve Equation
Console.WriteLine(matrixA.ConditionNumber()); // <<< Returns 5800 -> Big!
var r = matrixA.Solve(vectorB); // Existing code
A large result indicates an ill-conditioned problem. In this case a value of 5800 is returned, which is large. You might get better results using Gaussian Elimination with partial pivoting, but it still does not address the fact that the basic problem is ill-conditioned, which is why you get wildly incorrect answers.

How to Draw a Gaussian Curve in c#

I have a Histogram statistics bar chart with below data.
Count, HistogramBin
0, -1615.25
0, -1056.42
0, -497.48
1, 61.25
1, 620.05
1, 1178.92
0, 1737.76
0, 2296.59
I need to form Gauss curve based on above values. Could anyone guide me how to achieve the same.
I have written a function based on Wikipedia link: https://en.wikipedia.org/wiki/Gaussian_function
Our average is : 340.67
SD: Standard deviation: 488.98001098632812
private DataTable GenerateGaussTable1(DataTable histogramDataTable,
HistogramValueItem histogramValueDBItem)
{
double amplitude = (Average + 3 * Sigma) / 2;
double mean = Average;
double sd = Sigma;
DataTable dt = new DataTable();
dt.Columns.Add("x", typeof(float));
dt.Columns.Add("Y", typeof(float));
foreach (DataRow row in histogramDataTable.Rows)// top provided data
{
double x = Convert.ToDouble(row[1]) / 2;
double var1 = 1 / sd * Math.Sqrt(2 * 3.14);
double var2 = -0.5 * Math.Pow((x - mean)/sd, 2);
double var4= Math.Exp(var2);
double var5 = var1 * var4;
// Y = Amplitude * exp(-0.5 * ((X - Mean) / SD) ^ 2)
double y = var5;
dt.Rows.Add((float)x, (float)y);
}
return dt;
}
Here is my code:
double gauss(double x, double a, double b, double c)
{
var v1 = ( x - b) / (2d * c * c);
var v2 = -v1 * v1 / 2d;
var v3 = a * Math.Exp(v2);
return v3;
}
and:
private void button_Click(object sender, EventArgs e)
{
Series s1 = chart2.Series[0];
s1.ChartType = SeriesChartType.Line;
s1.Name = "Line";
Series s2 = chart2.Series.Add("Spline");
s2.ChartType = SeriesChartType.Spline;
double avg = 1.8;
double amp = 3;
double sd = 0.53;
List<double> xes = new List<double>
{ 0, 0, 0.05, 0.1, 0.4, 0.9, 1.3, 1.6, 2, 2.4, 2.8, 3.2, 4 };
foreach (var x in xes)
{
s1.Points.AddXY(x, gauss(x, amp, avg, sd));
s2.Points.AddXY(x, gauss(x, amp, avg, sd));
}
}
The math was taken from wikipedia
I think your SD is way too large to create a bell curve; try dividing by 10-100..! - Of course your SD actually is very large and so you really won't get a meaningful bell curve for those data..
I've tried your function, but it gives wrong curves,
The gauss function is wrong, why do you use "2d"?
Here the function :
so first, v1 = (x-b). Then v2 = (x-b)² / 2 c²
And finaly v3 = a exp (v2)
double gauss(double x, double a, double b, double c)
{
var v1 = (x - b);
var v2 = (v1 * v1) / (2 * (c*c));
var v3 = a * Math.Exp(-v2);
return v3;
}
After this fix, the curves are much better.

Calculating floating point error bound

I have geometrical algorithms and im struggling with floating point inaccuracies.
For example, I'm calculating wether a point lies on the left/right/on a plane (C#):
const double Epsilon = 1e-10;
internal double ComputeDistance(Vector3D v)
{
Vector3D normal = Plane.Normal;
Vertex v0 = Plane.Origin;
double a = normal.X;
double b = normal.Y;
double c = normal.Z;
double d = -(a * v0.X + b * v0.Y + c * v0.Z);
return a * v.X + b * v.Y + c * v.Z + d;
}
internal string DistanceSign(Vector3D v)
{
var distance = ComputeDistance(v);
return (distance > Epsilon ? "left" : (distance < -Epsilon ? "right" : "on"));
}
As shown in the code, I use a fixed Epsilon value.
But I don't trust this fixed epsilon because I don't know the size of the floating point error. If the fp error is bigger than my epsilon interval, then my algorithm will fail.
How can I make it robust? I have searched on the internet but haven't found a solution so far. I have read "What Every Computer Scientist Should Know About Floating-Point Arithmetic", it describes why fp errors occur but not how to solve them practically.
Edit
Here is a shewchuk predicate that doesn't seem work:
double[] pa = {0, 0};
double[] pb = {2 * Double.Epsilon, 0};
double[] pc = { 0, Double.Epsilon };
Assert.IsTrue(GeometricPredicates.Orient2D(pa, pb, pc) > 0);
The assertion fails because Orient2D return 0. The code is here
Edit2
Shouldn't it be possible to calculate an error bound by using the machine epsilon? According to wikipedia, the machine epsilon is an upper bound due to rounding. For double, it is 2^−53. So as I take it, when I have an arithmetic calculation:
double x = y + z
then the maximum error should be 2^−53. Shouldn't this fact enable the possiblity to calculate an appropriate epsilon? So two rewrite my method:
double const Machine_Eps = 1.11022302462516E-16 // (2^-53)
double Epsilon = Machine_Eps;
internal double ComputeDistance(Vector3D v)
{
Vector3D normal = Plane.Normal;
Vertex v0 = Plane.Origin;
double a = normal.X;
double b = normal.Y;
double c = normal.Z;
// 3 multiplications + 2 additions = maximum of 5*Machine_Eps
double d = -(a * v0.X + b * v0.Y + c * v0.Z);
// 3 multiplications + 3 additions = maximum of 6*Machine_Eps
Epsilon = 11 * Machine_Eps;
return a * v.X + b * v.Y + c * v.Z + d;
}
internal string DistanceSign(Vector3D v)
{
var distance = ComputeDistance(v);
return (distance > Epsilon ? "left" : (distance < -Epsilon ? "right" : "on"));
}
Ok now you can tell me how wrong I am. :)

Linq and Cross Products

I am trying to perform a Calculus Cross Product calculation using Linq and trying to figure out the pattern for the below code:
static void Main(string[] args)
{
double[] a = { 1, -1, -1 };
double[] b = {.5,1,.5};
var cross = from x in a
from y in b
select new {x,y};
List<double> LeftSide = new List<double>();
foreach (var c in cross) {
Console.WriteLine("x = " + c.x + " y = " + c.y);
double res = c.x * c.y;
Console.WriteLine("");
LeftSide.Add(res);
}
double i = LeftSide[5] - LeftSide[7];
double j = LeftSide[2] - LeftSide[6];
double k = LeftSide[1] - LeftSide[3];
Console.WriteLine("("+ i + "i) - (" + j + "j) +(" + k + "k)" );
Console.ReadLine();
}
Once I cross join the a and b, I need to perform the following calculations:
double i = LeftSide[5] - LeftSide[7];
double j = LeftSide[2] - LeftSide[6];
double k = LeftSide[1] - LeftSide[3];
This works and I get the desired output, put I know it can be written more efficiently. I am looking for any suggestions, to point me in the right direction.
Note: This is not a homework question, but is related to Calculus III Cross Products. I am a CS Major
You are making this way, way, way too complicated. The cross product of vectors (a0, a1, a2) and (b0, b1, b2) is (a1 * b2 - a2 * b1, a2 * b0 - a0 * b2, a0 * b1 - a1 * b0). So just compute that:
double[] a = { 1.0, -1.0, -1.0 };
double[] b = { 0.5, 1.0, 0.5 };
double[] cross =
{
a[1] * b[2] - a[2] * b[1],
a[2] * b[0] - a[0] * b[2],
a[0] * b[1] - a[1] * b[0]
};
And you're done in a single statement. There's no need to involve LINQ.
You are defining cross as a sequence of {x,y} elements, only to convert it to a sequence of x*y elements in the next step. This is redundant; you could generate your LeftSide immediately using:
double[] a = { 1, -1, -1 };
double[] b = { .5, 1, .5 };
var LeftSide =
(
from x in a
from y in b
select x * y
).ToArray();

Generate a random number in a Gaussian Range?

I want to use a random number generator that creates random numbers in a gaussian range where I can define the median by myself. I already asked a similar question here and now I'm using this code:
class RandomGaussian
{
private static Random random = new Random();
private static bool haveNextNextGaussian;
private static double nextNextGaussian;
public static double gaussianInRange(double from, double mean, double to)
{
if (!(from < mean && mean < to))
throw new ArgumentOutOfRangeException();
int p = Convert.ToInt32(random.NextDouble() * 100);
double retval;
if (p < (mean * Math.Abs(from - to)))
{
double interval1 = (NextGaussian() * (mean - from));
retval = from + (float)(interval1);
}
else
{
double interval2 = (NextGaussian() * (to - mean));
retval = mean + (float)(interval2);
}
while (retval < from || retval > to)
{
if (retval < from)
retval = (from - retval) + from;
if (retval > to)
retval = to - (retval - to);
}
return retval;
}
private static double NextGaussian()
{
if (haveNextNextGaussian)
{
haveNextNextGaussian = false;
return nextNextGaussian;
}
else
{
double v1, v2, s;
do
{
v1 = 2 * random.NextDouble() - 1;
v2 = 2 * random.NextDouble() - 1;
s = v1 * v1 + v2 * v2;
} while (s >= 1 || s == 0);
double multiplier = Math.Sqrt(-2 * Math.Log(s) / s);
nextNextGaussian = v2 * multiplier;
haveNextNextGaussian = true;
return v1 * multiplier;
}
}
}
Then to verify the results I plotted them with gaussianInRange(0, 0.5, 1) for n=100000000
As one can see the median is really at 0.5 but there isn't really a curve visible. So what I'm doing wrong?
EDIT
What i want is something like this where I can set the highest probability by myself by passing a value.
The simplest way to draw normal deviates conditional on them being in a particular range is with rejection sampling:
do {
retval = NextGaussian() * stdev + mean;
} while (retval < from || to < retval);
The same sort of thing is used when you draw coordinates (v1, v2) in a circle in your unconditional normal generator.
Simply folding in values outside the range doesn't produce the same distribution.
Also, if you have a good implementation of the error function and its inverse, you can calculate the values directly using an inverse CDF. The CDF of a normal distribution is
F(retval) = (1 + erf((retval-mean) / (stdev*sqrt(2)))) / 2
The CDF of a censored distribution is
C(retval) = (F(retval) - F(from)) / (F(to) - F(from)), from ≤ x < to
To draw a random number using a CDF, you draw v from a uniform distribution on [0, 1] and solve C(retval) = v. This gives
double v = random.NextDouble();
double t1 = erf((from - mean) / (stdev*sqrt(2)));
t2 = erf((to - mean) / (stdev*sqrt(2)));
double retval = mean + stdev * sqrt(2) * erf_inv(t1*(1-v) + t2*v);
You can precalculate t1 and t2 for specific parameters. The advantage of this approach is that there is no rejection sampling, so you only need a single NextDouble() per draw. If the [from, to] interval is small this will be faster.
However, it sounds like you might want the binomial distribution instead.
I have similar methods in my Graph generator (had to modify it a bit):
Returns a random floating-point number using a generator function with a specific range:
private double NextFunctional(Func<double, double> func, double from, double to, double height, out double x)
{
double halfWidth = (to - from) / 2;
double distance = halfWidth + from;
x = this.rand.NextDouble() * 2 - 1;// -1 .. 1
double y = func(x);
x = halfWidth * x + distance;
y *= height;
return y;
}
Gaussian function:
private double Gauss(double x)
{
// Graph should look better with double-x scale.
x *= 2;
double σ = 1 / Math.Sqrt(2 * Math.PI);
double variance = Math.Pow(σ, 2);
double exp = -0.5 * Math.Pow(x, 2) / variance;
double y = 1 / Math.Sqrt(2 * Math.PI * variance) * Math.Pow(Math.E, exp);
return y;
}
A method that generates a graph using the random numbers:
private void PlotGraph(Graphics g, Pen p, double from, double to, double height)
{
for (int i = 0; i < 1000; i++)
{
double x;
double y = this.NextFunctional(this.Gauss, from, to, height, out x);
this.DrawPoint(g, p, x, y);
}
}
I would rather used a cosine function - it is much faster and pretty close to the gaussian function for your needs:
double x;
double y = this.NextFunctional(a => Math.Cos(a * Math.PI), from, to, height, out x);
The out double x parameter in the NextFunctional() method is there so you can easily test it on your graphs (I use an iterator in my method).

Categories