public override Coordinates <Matrix> calculatePose(Constellation constellation)
{
int CONSTELLATION_SIZE = constellation.getUsedConstellationSize();
// Initialize matrices for data storage
SimpleMatrix <Matrix> rxPosSimpleVector = Constellation.getRxPosAsVector(constellation.getRxPos());
SimpleMatrix <Matrix> satPosMat = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 3);
SimpleMatrix <Matrix> tropoCorr = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 1);
SimpleMatrix <Matrix> svClkBias = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 1);
SimpleMatrix <Matrix> ionoCorr = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 1);
SimpleMatrix <Matrix> shapiroCorr = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 1);
SimpleMatrix <Matrix> sigma2 = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 1);
SimpleMatrix <Matrix> prVect = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 1);
SimpleMatrix <Matrix> vcvMeasurement;
SimpleMatrix <Matrix> PDOP = new SimpleMatrix <Matrix>(1, 1);
double elevation, measVar, measVarC1;
int CN0;
TopocentricCoordinates <Matrix> topo = new TopocentricCoordinates <Matrix>();
Coordinates <Matrix> origin; // = new Coordinates<Matrix>(){};
Coordinates <Matrix> target; // = new Coordinates<Matrix>(){};
///////////////////////////// SV Coordinates<Matrix>/velocities + PR corrections computation ////////////////////////////////////////////////////
try
{
for (int ii = 0; ii < CONSTELLATION_SIZE; ii++)
{
// Set the measurements into a vector
prVect.set(ii, constellation.getSatellite(ii).getPseudorange());
// Compute the satellite Coordinates<Matrix>
svClkBias.set(ii, constellation.getSatellite(ii).getClockBias());
///////////////////////////// PR corrections computations ////////////////////////////////////////////////////
// Assign the computed SV Coordinates<Matrix> into a matrix
satPosMat.set(ii, 0, constellation.getSatellite(ii).getSatellitePosition().getX());
satPosMat.set(ii, 1, constellation.getSatellite(ii).getSatellitePosition().getY());
satPosMat.set(ii, 2, constellation.getSatellite(ii).getSatellitePosition().getZ());
// Compute the elevation and azimuth angles for each satellite
origin = Coordinates <Matrix> .globalXYZInstance(rxPosSimpleVector.get(0), rxPosSimpleVector.get(1), rxPosSimpleVector.get(2));
target = Coordinates <Matrix> .globalXYZInstance(satPosMat.get(ii, 0), satPosMat.get(ii, 1), satPosMat.get(ii, 2));
// origin.setXYZ(rxPosSimpleVector.get(0), rxPosSimpleVector.get(1),rxPosSimpleVector.get(2));
// target.setXYZ(satPosMat.get(ii, 0),satPosMat.get(ii, 1),satPosMat.get(ii, 2) );
topo.computeTopocentric(origin, target);
elevation = topo.getElevation() * (Math.PI / 180.0);
// Set the variance of the measurement for each satellite
measVar = sigma2Meas * Math.Pow(a + b * Math.Exp(-elevation / 10.0), 2);
sigma2.set(ii, measVar);
///////////////////////////// Printing results ////////////////////////////////////////////////////
// Print the computed satellite Coordinates<Matrix> \ velocities
/*
* System.out.println();
* System.out.println();
* System.out.println("The GPS ECEF Coordinates<Matrix> of " + "SV" + constellation.getSatellite(ii).getSatId() + " are:");
* System.out.printf("%.4f", satPosMat.get(ii, 0));
* System.out.println();
* System.out.printf("%.4f", satPosMat.get(ii, 1));
* System.out.println();
* System.out.printf("%.4f", satPosMat.get(ii, 2));
* System.out.println();
* System.out.printf("Tropo corr: %.4f", tropoCorr.get(ii));
* System.out.println();
* System.out.printf("Iono corr: %.4f", ionoCorr.get(ii));
* System.out.println();
*/
//System.out.printf("Shapiro corr: %.4f", shapiroCorr.get(ii));
}
}
catch (Exception e)
{
//e.printStackTrace();
//if (e.getClass() == IndexOutOfBoundsException.class){
// Log.e(TAG, "calculatePose: Satellites cleared before calculating result!");
//}
constellation.setRxPos(ZERO_POSE); // Right at the edge of the plot
rxPosSimpleVector = Constellation.getRxPosAsVector(constellation.getRxPos());
return(Coordinates <Matrix> .globalXYZInstance(rxPosSimpleVector.get(0), rxPosSimpleVector.get(1), rxPosSimpleVector.get(2)));
}
/*
* [WEIGHTED LEAST SQUARES] Determination of the position + clock bias
*/
try
{
// VCV matrix of the pseudorange measurements
vcvMeasurement = ((SimpleBase <SimpleMatrix <Matrix>, Matrix>)sigma2).diag();
SimpleMatrix <Matrix> W = vcvMeasurement.invert();
// Initialization of the required matrices and vectors
SimpleMatrix <Matrix> xHat = new SimpleMatrix <Matrix>(4, 1); // vector holding the WLS estimates
SimpleMatrix <Matrix> z = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 1); // observation vector
SimpleMatrix <Matrix> H = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 4); // observation matrix
SimpleMatrix <Matrix> distPred = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 1); // predicted distances
SimpleMatrix <Matrix> measPred = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 1); // predicted measurements
// Start the estimation (10 loops)
for (int iter = 0; iter < NUMBER_OF_ITERATIONS; iter++)
{
// Calculation of the components within the observation matrix (H)
for (int k = 0; k < CONSTELLATION_SIZE; k++)
{
// Computation of the geometric distance
distPred.set(k, Math.Sqrt(
Math.Pow(satPosMat.get(k, 0) - rxPosSimpleVector.get(0), 2)
+ Math.Pow(satPosMat.get(k, 1) - rxPosSimpleVector.get(1), 2)
+ Math.Pow(satPosMat.get(k, 2) - rxPosSimpleVector.get(2), 2)
));
// Measurement prediction
measPred.set(k, distPred.get(k)
+ constellation.getSatellite(k).getAccumulatedCorrection() - svClkBias.get(k));
// Compute the observation matrix (H)
H.set(k, 0, (constellation.getRxPos().getX() - satPosMat.get(k, 0)) / distPred.get(k));
H.set(k, 1, (constellation.getRxPos().getY() - satPosMat.get(k, 1)) / distPred.get(k));
H.set(k, 2, (constellation.getRxPos().getZ() - satPosMat.get(k, 2)) / distPred.get(k));
H.set(k, 3, 1.0);
}
// Compute the prefit vector (z)
z.set(prVect.minus(measPred));
// Estimate the unknowns (dxHat)
SimpleMatrix <Matrix> Cov = H.transpose().mult(W).mult(H);
SimpleMatrix <Matrix> CovDOP = H.transpose().mult(H);
SimpleMatrix <Matrix> dopMatrix = CovDOP.invert();
xHat.set(Cov.invert().mult(H.transpose()).mult(W).mult(z));
PDOP.set(Math.Sqrt(dopMatrix.get(0, 0) + dopMatrix.get(1, 1)));
// Update the receiver position
// rxPosSimpleVector.set(rxPosSimpleVector.plus(xHat));
rxPosSimpleVector.set(0, rxPosSimpleVector.get(0) + xHat.get(0));
rxPosSimpleVector.set(1, rxPosSimpleVector.get(1) + xHat.get(1));
rxPosSimpleVector.set(2, rxPosSimpleVector.get(2) + xHat.get(2));
rxPosSimpleVector.set(3, xHat.get(3));
}
clockBias = rxPosSimpleVector.get(3);
}
catch (Exception e)
{
//if (e.getClass() == IndexOutOfBoundsException.class){
// Log.e(TAG, "calculatePose: Satellites cleared before calculating result!");
//} else if (e.getClass() == SingularMatrixException.class) {
// Log.e(TAG, "calculatePose: SingularMatrixException caught!");
//}
constellation.setRxPos(ZERO_POSE); // Right at the edge of the plot
rxPosSimpleVector = Constellation.getRxPosAsVector(constellation.getRxPos());
//e.printStackTrace();
}
//System.out.println();
// Print the estimated receiver position
// rxPosSimpleVector.print();
Log.Debug(TAG, "calculatePose: rxPosSimpleVector (ECEF): " + rxPosSimpleVector.get(0) + ", " + rxPosSimpleVector.get(1) + ", " + rxPosSimpleVector.get(2) + ";");
Coordinates <Matrix> pose = Coordinates <Matrix> .globalXYZInstance(rxPosSimpleVector.get(0), rxPosSimpleVector.get(1), rxPosSimpleVector.get(2));
Log.Debug(TAG, "calculatePose: pose (ECEF): " + pose.getX() + ", " + pose.getY() + ", " + pose.getZ() + ";");
Log.Debug(TAG, "calculatePose: pose (lat-lon): " + pose.getGeodeticLatitude() + ", " + pose.getGeodeticLongitude() + ", " + pose.getGeodeticHeight() + ";");
Log.Debug(TAG, "calculated PDOP:" + PDOP.get(0) + ";");
return(pose);
}
public override void calculateCorrection(Time currentTime, Coordinates <Matrix> approximatedPose, SatellitePosition satelliteCoordinates, NavigationProducer navigationProducer, Location initialLocation)
{
// Compute also the geodetic version of the user position (latitude, longitude, height)
approximatedPose.computeGeodetic();
// Get the user's height
double height = approximatedPose.getGeodeticHeight();
// Compute the elevation and azimuth angles for each satellite
TopocentricCoordinates <Matrix> topo = new TopocentricCoordinates <Matrix>();
topo.computeTopocentric(approximatedPose, satelliteCoordinates);
// Assign the elevation information to a new variable
double elevation = topo.getElevation();
double tropoCorr = 0;
if (height > 5000)
{
return;
}
elevation = Math.Abs(elevation) / 180.0 * Math.PI;
if (elevation == 0)
{
elevation = elevation + 0.01;
}
// Numerical constants and tables for Saastamoinen algorithm
// (troposphere correction)
double hr = 50.0;
int[] ha = { 0, 500, 1000, 1500, 2000, 2500, 3000, 4000, 5000 };
double[] ba = { 1.156, 1.079, 1.006, 0.938, 0.874, 0.813, 0.757, 0.654, 0.563 };
// Saastamoinen algorithm
double P = Constants.STANDARD_PRESSURE * Math.Pow((1 - 0.0000226 * height), 5.225);
double T = Constants.STANDARD_TEMPERATURE - 0.0065 * height;
double H = hr * Math.Exp(-0.0006396 * height);
// If height is below zero, keep the maximum correction value
double B = ba[0];
// Otherwise, interpolate the tables
if (height >= 0)
{
int i = 1;
while (height > ha[i])
{
i++;
}
double m = (ba[i] - ba[i - 1]) / (ha[i] - ha[i - 1]);
B = ba[i - 1] + m * (height - ha[i - 1]);
}
double e = 0.01
* H
* Math.Exp(-37.2465 + 0.213166 * T - 0.000256908
* Math.Pow(T, 2));
tropoCorr = ((0.002277 / Math.Sin(elevation))
* (P - (B / Math.Pow(Math.Tan(elevation), 2))) + (0.002277 / Math.Sin(elevation))
* (1255 / T + 0.05) * e);
correctionValue = tropoCorr;
}
public override void calculateCorrection(Time currentTime, Coordinates <Matrix> approximatedPose, SatellitePosition satelliteCoordinates, NavigationProducer navigationProducer, Location initialLocation)
{
DateTime foo = DateTime.Now;
long unixTime = ((DateTimeOffset)foo).ToUnixTimeSeconds();
IonoGps iono = navigationProducer.getIono(unixTime, initialLocation);
if (iono.getBeta(0) == 0)
{
correctionValue = 0.0;
}
else
{
// Compute the elevation and azimuth angles for each satellite
TopocentricCoordinates <Matrix> topo = new TopocentricCoordinates <Matrix>();
topo.computeTopocentric(approximatedPose, satelliteCoordinates);
// Assign the elevation and azimuth information to new variables
double elevation = topo.getElevation();
double azimuth = topo.getAzimuth();
double ionoCorr = 0;
if (iono == null)
{
return;
}
// double a0 = navigation.getIono(currentTime.getMsec(),0);
// double a1 = navigation.getIono(currentTime.getMsec(),1);
// double a2 = navigation.getIono(currentTime.getMsec(),2);
// double a3 = navigation.getIono(currentTime.getMsec(),3);
// double b0 = navigation.getIono(currentTime.getMsec(),4);
// double b1 = navigation.getIono(currentTime.getMsec(),5);
// double b2 = navigation.getIono(currentTime.getMsec(),6);
// double b3 = navigation.getIono(currentTime.getMsec(),7);
elevation = Math.Abs(elevation);
// Parameter conversion to semicircles
double lon = approximatedPose.getGeodeticLongitude() / 180; // geod.get(0)
double lat = approximatedPose.getGeodeticLatitude() / 180; //geod.get(1)
azimuth = azimuth / 180;
elevation = elevation / 180;
// Klobuchar algorithm
// Compute the slant factor
double f = 1 + 16 * Math.Pow((0.53 - elevation), 3);
// Compute the earth-centred angle
double psi = 0.0137 / (elevation + 0.11) - 0.022;
// Compute the latitude of the Ionospheric Pierce Point (IPP)
double phi = lat + psi * Math.Cos(azimuth * Math.PI);
if (phi > 0.416)
{
phi = 0.416;
}
if (phi < -0.416)
{
phi = -0.416;
}
// Compute the longitude of the IPP
double lambda = lon + (psi * Math.Sin(azimuth * Math.PI))
/ Math.Cos(phi * Math.PI);
// Find the geomagnetic latitude of the IPP
double ro = phi + 0.064 * Math.Cos((lambda - 1.617) * Math.PI);
// Find the local time at the IPP
double t = lambda * 43200 + unixTime;
while (t >= 86400)
{
t = t - 86400;
}
while (t < 0)
{
t = t + 86400;
}
// Compute the period of ionospheric delay
double p = iono.getBeta(0) + iono.getBeta(1) * ro + iono.getBeta(2) * Math.Pow(ro, 2) + iono.getBeta(3) * Math.Pow(ro, 3);
if (p < 72000)
{
p = 72000;
}
// Compute the amplitude of ionospheric delay
double a = iono.getAlpha(0) + iono.getAlpha(1) * ro + iono.getAlpha(2) * Math.Pow(ro, 2) + iono.getAlpha(3) * Math.Pow(ro, 3);
if (a < 0)
{
a = 0;
}
// Compute the phase of ionospheric delay
double x = (2 * Math.PI * (t - 50400)) / p;
// Compute the ionospheric correction
if (Math.Abs(x) < 1.57)
{
ionoCorr = Constants.SPEED_OF_LIGHT
* f
* (5e-9 + a
* (1 - (Math.Pow(x, 2)) / 2 + (Math.Pow(x, 4)) / 24));
}
else
{
ionoCorr = Constants.SPEED_OF_LIGHT * f * 5e-9;
}
correctionValue = ionoCorr;
}
}
public Task <bool> SlewToCoordinatesAsync(TopocentricCoordinates coords)
{
return(handler.SlewToCoordinatesAsync(coords));
}
