/// <summary>
/// Solves the navigation problem by the range-difference method.
/// </summary>
/// <param name="receivers">Matrix of vectors { X, Y, Z }</param>
/// <param name="time">Vector of time</param>
/// <returns>Vector { X, Y, Z }</returns>
public double[] Solve(double[][] receivers, double[] time)
{
int length = receivers.GetLength(0);
if (length < 2)
{
throw new Exception("Number of receivers must be greater than 1");
}
else if (length < 5)
{
return(RDMS.RDM4(receivers, time, this.eps));
}
else if (length == 5)
{
return(RDMS.RDM5(receivers, time));
}
else
{
int dim = 5;
double[][] packet = new double[dim][];
double[] timerp = new double[dim];
for (int i = 0; i < dim; i++)
{
packet[i] = receivers[i];
timerp[i] = time[i];
}
return(RDMS.RDM5(packet, timerp));
}
}
/// <summary>
/// Solves the navigation problem by the range-difference method (nonlinear method).
/// </summary>
/// <param name="A">Matrix of vectors { X, Y, Z }</param>
/// <param name="T">Vector of time</param>
/// <returns>Vector { X, Y, Z }</returns>
/// <param name="eps">Epsilon (0, 1)</param>
private static double[] RDM4(double[][] A, double[] T, double eps = 1e-8)
{
// Params
int count = A.GetLength(0);
double[] V = new double[count];
double[][] B;
double[] F, S;
// Roots
for (int i = 0; i < maxIterations; i++)
{
// Ax = b
B = RDMS.Left(A, T, V);
F = RDMS.Right(A, T, V);
S = Vector.Solve(B, F);
V = Vector.Add(V, S);
// Stop point
if (RDMS.Convergence(S, eps))
{
break;
}
}
// Vector { X, Y, Z }
return(Vector.Resize(V, 3));
}
// **************************************************************
// ALGORITHMS
// **************************************************************
// * RDM implementation for 5 (or more) time-synchronized
// receivers by solving a linearized system of equations [1].
// --------------------------------------------------------------
// * RDM implementation for 2, 3 and 4 time-synchronized
// receivers by solving a nonlinear system of equations [2,3].
//
//
// **************************************************************
// REFERENCES
// **************************************************************
// [1] I.V. Grin, R.A. Ershov, O.A. Morozov, V.R. Fidelman -
// "Evaluation of radio source’s coordinates based on solution
// of linearized system of equations by range-difference method"
// --------------------------------------------------------------
// [2] V.B. Burdin, V.A. Tyurin, S.A. Tyurin, V.M. Asiryan -
// "The estimation of target positioning by means of the
// range-difference method"
// --------------------------------------------------------------
// [3] E.P. Voroshilin, M.V. Mironov, V.A. Gromov -
// "The estimation of radio source positioning by means of the
// range-difference method using the multiposition passive
// satellite system"
// **************************************************************
/// <summary>
/// Solves the navigation problem by the range-difference method (linear method).
/// </summary>
/// <param name="A">Matrix of vectors { X, Y, Z }</param>
/// <param name="T">Vector of time</param>
/// <returns>Vector { X, Y, Z }</returns>
private static double[] RDM5(double[][] A, double[] T)
{
// Roots
double[][] B = RDMS.Left(A, T);
double[] F = RDMS.Right(A, T);
double[] S = Vector.Solve(B, F);
// Vector { X, Y, Z }
return(Vector.Resize(S, 3));
}
/// <summary>
/// Returns a matrix of receive points.
/// </summary>
/// <param name="vector">Vector { X, Y, Z }</param>
/// <param name="scaling">Scaling vector { X, Y, Z }</param>
/// <param name="sigma">Standard deviation</param>
/// <param name="count">Number of receivers (>1)</param>
/// <returns>Matrix of vectors { X, Y, Z }</returns>
public static double[][] GetReceiver(double[] vector, double[] scaling, double sigma, int count)
{
// compute
double[][] R = new double[count][];
for (int i = 0; i < count; i++)
{
R[i] = RDMS.GetReceiver(vector, scaling, sigma);
}
return(R);
}
/// <summary>
/// Returns the propagation time of a wave between matrix of receivers and target vector.
/// </summary>
/// <param name="receivers">Matrix of vectors { X, Y, Z }</param>
/// <param name="vector">Vector { X, Y, Z }</param>
/// <returns>Vector { T0, T1, T2, T3 }</returns>
public static double[] GetTime(double[][] receivers, double[] vector)
{
int length = receivers.GetLength(0);
double[] c = new double[length];
for (int i = 0; i < length; i++)
{
c[i] = RDMS.GetTime(receivers[i], vector);
}
return(c);
}
/// <summary>
/// Returns a target vector.
/// </summary>
/// <param name="receivers">Matrix of vectors { X, Y, Z }</param>
/// <param name="sigma">Standard deviation</param>
/// <param name="count">Number of targets</param>
/// <returns>Vector { X, Y, Z }</returns>
public static double[][] GetTarget(double[][] receivers, double sigma, int count)
{
double[] max, min;
RDMS.GetExtremum(receivers, out min, out max);
double[][] targets = new double[count][];
for (int i = 0; i < count; i++)
{
targets[i] = RDMS.GetTarget(min, max, sigma);
}
return(targets);
}
/// <summary>
/// Returns the propagation time of a wave between matrix of receivers and target vector.
/// </summary>
/// <param name="receivers">Matrix of vectors { X, Y, Z }</param>
/// <param name="vector">Vector { X, Y, Z }</param>
/// <returns>Vector { T0, T1, T2, T3 }</returns>
public static double[][] GetTime(double[][] receivers, double[][] vector)
{
int count = vector.GetLength(0);
int length = receivers.GetLength(0);
double[][] c = new double[count][];
for (int i = 0; i < count; i++)
{
c[i] = new double[length];
for (int j = 0; j < length; j++)
{
c[i][j] = RDMS.GetTime(receivers[j], vector[i]);
}
}
return(c);
}
/// <summary>
/// Returns a target vector.
/// </summary>
/// <param name="receivers">Matrix of vectors { X, Y, Z }</param>
/// <param name="sigma">Standard deviation</param>
/// <returns>Vector { X, Y, Z }</returns>
public static double[] GetTarget(double[][] receivers, double sigma)
{
double[] max, min;
RDMS.GetExtremum(receivers, out min, out max);
return(RDMS.GetTarget(min, max, sigma));
}
