80 lines
2.3 KiB
C#
80 lines
2.3 KiB
C#
namespace omp
|
|
{
|
|
public class OverlappingPoints : Exception { }
|
|
|
|
public static class Calculator
|
|
{
|
|
public const double StationPDOP = -1000;
|
|
|
|
public static double[,] Jacobian1T(List<Point> stations, Point position)
|
|
{
|
|
int n = stations.Count;
|
|
double[,] J = new double[4, n];
|
|
|
|
for (int i = 0; i < n; ++i)
|
|
{
|
|
double distance = position.Distance(stations[i]);
|
|
|
|
if (distance == 0)
|
|
{
|
|
throw new OverlappingPoints();
|
|
}
|
|
|
|
J[0, i] = -(stations[i].x - position.x) / distance;
|
|
J[1, i] = -(stations[i].y - position.y) / distance;
|
|
J[2, i] = -(stations[i].z - position.z) / distance;
|
|
J[3, i] = 1; // Clock bias
|
|
}
|
|
|
|
return J;
|
|
}
|
|
|
|
public static double PDOP(List<Point> stations, Point position)
|
|
{
|
|
double[,] JR = Jacobian1T(stations, position);
|
|
|
|
double[,] mult = MatrixOps.MultTrans(JR);
|
|
|
|
double det = MatrixOps.Det(mult);
|
|
|
|
double sx2 = MatrixOps.Adj(mult, 0, 0) / det;
|
|
double sy2 = MatrixOps.Adj(mult, 1, 1) / det;
|
|
double sz2 = MatrixOps.Adj(mult, 2, 2) / det;
|
|
|
|
return Math.Sqrt(sx2 + sy2 + sz2);
|
|
}
|
|
|
|
public static void PDOPs(double[,] pdops, Configurator conf, out double min, out double max)
|
|
{
|
|
min = double.MaxValue;
|
|
max = double.MinValue;
|
|
|
|
for (int i = 0; i < pdops.GetLength(0); ++i)
|
|
{
|
|
for (int j = 0; j < pdops.GetLength(1); ++j)
|
|
{
|
|
try
|
|
{
|
|
pdops[i, j] = PDOP(conf.stations, new Point(i, j, conf.zCoordinate));
|
|
|
|
if (pdops[i, j] > max)
|
|
{
|
|
max = pdops[i, j];
|
|
}
|
|
|
|
if (pdops[i, j] < min)
|
|
{
|
|
min = pdops[i, j];
|
|
}
|
|
}
|
|
catch (OverlappingPoints)
|
|
{
|
|
pdops[i, j] = StationPDOP;
|
|
}
|
|
}
|
|
}
|
|
|
|
Console.WriteLine("{0}...{1}", min, max);
|
|
}
|
|
}
|
|
} |