private SimpleMatrix <Matrix> satellite_motion_diff_eq(SimpleMatrix <Matrix> pos1Array,
SimpleMatrix <Matrix> vel1Array, SimpleMatrix <Matrix> accArray, long ellAGlo,
double gmGlo, double j2Glo, double omegaeDotGlo)
{
// TODO Auto-generated method stub
/* renaming variables for better readability position */
double X = pos1Array.get(0);
double Y = pos1Array.get(1);
double Z = pos1Array.get(2);
// System.out.println("X: " + X);
// System.out.println("Y: " + Y);
// System.out.println("Z: " + Z);
/* velocity */
double Xv = vel1Array.get(0);
double Yv = vel1Array.get(1);
// System.out.println("Xv: " + Xv);
// System.out.println("Yv: " + Yv);
/* acceleration (i.e. perturbation) */
double Xa = accArray.get(0);
double Ya = accArray.get(1);
double Za = accArray.get(2);
// System.out.println("Xa: " + Xa);
// System.out.println("Ya: " + Ya);
// System.out.println("Za: " + Za);
/* parameters */
double r = Math.Sqrt(Math.Pow(X, 2) + Math.Pow(Y, 2) + Math.Pow(Z, 2));
double g = -gmGlo / Math.Pow(r, 3);
double h = j2Glo * 1.5 * Math.Pow((ellAGlo / r), 2);
double k = 5 * Math.Pow(Z, 2) / Math.Pow(r, 2);
// System.out.println("r: " + r);
// System.out.println("g: " + g);
// System.out.println("h: " + h);
// System.out.println("k: " + k);
/* differential velocity */
double[] vel_dot = new double[3];
vel_dot[0] = g * X * (1 - h * (k - 1)) + Xa + Math.Pow(omegaeDotGlo, 2) * X + 2 * omegaeDotGlo * Yv;
// System.out.println("vel1: " + vel_dot[0]);
vel_dot[1] = g * Y * (1 - h * (k - 1)) + Ya + Math.Pow(omegaeDotGlo, 2) * Y - 2 * omegaeDotGlo * Xv;
// System.out.println("vel2: " + vel_dot[1]);
vel_dot[2] = g * Z * (1 - h * (k - 3)) + Za;
// System.out.println("vel3: " + vel_dot[2]);
SimpleMatrix <Matrix> velDotArray = new SimpleMatrix <Matrix>(1, 3, true, vel_dot);
// velDotArray.print();
return(velDotArray);
}
public Object clone()
{
SatellitePosition sp = new SatellitePosition(this.unixTime, this.satID, this.satType, this.getX(), this.getY(), this.getZ());
sp.maneuver = this.maneuver;
sp.predicted = this.predicted;
sp.satelliteClockError = this.satelliteClockError;
sp.setSpeed(speed.get(0), speed.get(1), speed.get(2));
return(sp);
}
/**
* Computes the QR decomposition of the provided matrix.
*
* @param A Matrix which is to be decomposed. Not modified.
*/
public void decompose(SimpleMatrix <DMatrixRMaj> A)
{
this.QR = A.copy() as SimpleMatrix <DMatrixRMaj>;
int N = Math.Min(A.numCols(), A.numRows());
gammas = new double[A.numCols()];
for (int i = 0; i < N; i++)
{
// use extract matrix to get the column that is to be zeroed
SimpleMatrix <DMatrixRMaj> v = QR.extractMatrix(i, SimpleMatrix <DMatrixRMaj> .END, i, i + 1) as SimpleMatrix <DMatrixRMaj>;
double max = v.elementMaxAbs();
if (max > 0 && v.getNumElements() > 1)
{
// normalize to reduce overflow issues
v = v.divide(max) as SimpleMatrix <DMatrixRMaj>;
// compute the magnitude of the vector
double tau = v.normF();
if (v.get(0) < 0)
{
tau *= -1.0;
}
double u_0 = v.get(0) + tau;
double gamma = u_0 / tau;
v = v.divide(u_0) as SimpleMatrix <DMatrixRMaj>;
v.set(0, 1.0);
// extract the submatrix of A which is being operated on
SimpleBase <DMatrixRMaj> A_small = QR.extractMatrix(i, SimpleMatrix <DMatrixRMaj> .END, i, SimpleMatrix <DMatrixRMaj> .END);
// A = (I - γ*u*u<sup>T</sup>)A
A_small = A_small.plus(-gamma, v.mult(v.transpose()).mult(A_small));
// save the results
QR.insertIntoThis(i, i, A_small);
QR.insertIntoThis(i + 1, i, v.extractMatrix(1, SimpleMatrix <DMatrixRMaj> .END, 0, 1));
// Alternatively, the two lines above can be replaced with in-place equations
// READ THE JAVADOC TO UNDERSTAND HOW THIS WORKS!
// QR.equation("QR(i:,i:) = A","QR",i,"i",A_small,"A");
// QR.equation("QR((i+1):,i) = v(1:,0)","QR",i,"i",v,"v");
// save gamma for recomputing Q later on
gammas[i] = gamma;
}
}
}
/**
* @param origin
*/
public TopocentricCoordinates <W> computeTopocentric(Coordinates <W> origin, Coordinates <W> target)
{
// // Build rotation matrix from global to local reference systems
// SimpleMatrix R = globalToLocalMatrix(origin);
//
// // Compute local vector from origin to this object coordinates
// //SimpleMatrix enu = R.mult(target.ecef.minus(origin.ecef));
// SimpleMatrix enu = R.mult(target.minusXYZ(origin));
origin.computeLocalV2(target);
double E = origin.getE(); //enu.get(0);
double N = origin.getN(); //enu.get(1);
double U = origin.getU(); //enu.get(2);
// Compute horizontal distance from origin to this object
double hDist = Math.Sqrt(Math.Pow(E, 2) + Math.Pow(N, 2));
// If this object is at zenith ...
if (hDist < 1e-20)
{
// ... set azimuth = 0 and elevation = 90, ...
topocentric.set(0, 0, 0);
topocentric.set(1, 0, 90);
}
else
{
// ... otherwise compute azimuth ...
topocentric.set(0, 0, Math.Atan2(E, N) / Math.PI * 180.0);
// ... and elevation
topocentric.set(1, 0, Math.Atan2(U, hDist) / Math.PI * 180.0);
if (topocentric.get(0) < 0)
{
topocentric.set(0, 0, topocentric.get(0) + 360);
}
}
// Compute distance
topocentric.set(2, 0, Math.Sqrt(Math.Pow(E, 2) + Math.Pow(N, 2) + Math.Pow(U, 2)));
return(this);
}
public override void calculateCorrection(Time currentTime, Coordinates <Matrix> approximatedPose, SatellitePosition satelliteCoordinates, NavigationProducer navigationProducer, Location initialLocation)
{
// Compute the difference vector between the receiver and the satellite
SimpleMatrix <Matrix> diff = approximatedPose.minusXYZ(satelliteCoordinates);
// Compute the geometric distance between the receiver and the satellite
double geomDist = Math.Sqrt(Math.Pow(diff.get(0), 2) + Math.Pow(diff.get(1), 2) + Math.Pow(diff.get(2), 2));
// Compute the geocentric distance of the receiver
double geoDistRx = Math.Sqrt(Math.Pow(approximatedPose.getX(), 2) + Math.Pow(approximatedPose.getY(), 2) + Math.Pow(approximatedPose.getZ(), 2));
// Compute the geocentric distance of the satellite
double geoDistSv = Math.Sqrt(Math.Pow(satelliteCoordinates.getX(), 2) + Math.Pow(satelliteCoordinates.getY(), 2) + Math.Pow(satelliteCoordinates.getZ(), 2));
// Compute the shapiro correction
correctionValue = ((2.0 * Constants.EARTH_GRAVITATIONAL_CONSTANT) / Math.Pow(Constants.SPEED_OF_LIGHT, 2)) * Math.Log((geoDistSv + geoDistRx + geomDist) / (geoDistSv + geoDistRx - geomDist));
}
/**
* initialize process noise matrix Q using receiver clock frequency and phase errors, DELTA_T
* and the noise on the position S_xy and S_z
*/
private void initQ()
{
Q.set(idxX, idxX, Math.Pow(S_xy, 2.0) * Math.Pow(DELTA_T, 3) / 3.0);
Q.set(idxU, idxX, Math.Pow(S_xy, 2.0) * Math.Pow(DELTA_T, 2) / 2.0);
Q.set(idxX, idxU, Q.get(idxU, idxX)); // assure symmetry of matrix
Q.set(idxU, idxU, Math.Pow(S_xy, 2.0) * DELTA_T);
Q.set(idxY, idxY, Math.Pow(S_xy, 2.0) * Math.Pow(DELTA_T, 3) / 3.0);
Q.set(idxV, idxY, Math.Pow(S_xy, 2.0) * Math.Pow(DELTA_T, 2) / 2.0);
Q.set(idxY, idxV, Q.get(idxV, idxY)); // symmetry
Q.set(idxV, idxV, Math.Pow(S_xy, 2.0) * DELTA_T);
Q.set(idxZ, idxZ, Math.Pow(S_z, 2.0) * Math.Pow(DELTA_T, 3) / 3.0);
Q.set(idxZ, idxW, Math.Pow(S_z, 2.0) * Math.Pow(DELTA_T, 2) / 2.0);
Q.set(idxW, idxZ, Q.get(idxZ, idxW)); // symmetry
Q.set(idxW, idxW, Math.Pow(S_z, 2.0) * DELTA_T);
// Tuning of the process noise matrix (Q)
Q.set(idxClockBias, idxClockBias, S_f + S_g * Math.Pow(DELTA_T, 3) / 3.0);
Q.set(idxClockBias, idxClockDrift, S_g * Math.Pow(DELTA_T, 2) / 2.0);
Q.set(idxClockDrift, idxClockBias, S_g * Math.Pow(DELTA_T, 2) / 2.0);
Q.set(idxClockDrift, idxClockDrift, S_g * DELTA_T);
}
/**
* Returns the R matrix.
*/
public SimpleMatrix <DMatrixRMaj> getR()
{
SimpleMatrix <DMatrixRMaj> R = new SimpleMatrix <DMatrixRMaj>(QR.numRows(), QR.numCols());
int N = Math.Min(QR.numCols(), QR.numRows());
for (int i = 0; i < N; i++)
{
for (int j = i; j < QR.numCols(); j++)
{
R.set(i, j, QR.get(i, j));
}
}
return(R);
}
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 double getClockBias()
{
return(x_meas.get(idxClockBias));
}
public override Coordinates <Matrix> calculatePose(Constellation constellation)
{
/** number of satellites in constellation
*/
int CONSTELLATION_SIZE = constellation.getUsedConstellationSize();
/** innovation sequence vector
*/
SimpleMatrix <Matrix> gamma;
// Initialize a variable to hold the predicted (geometric) distance towards each satellite
/** geometric distance in meters to every satellite. internal variable.
*/
double distPred = 0.0; // geometric Distance
/** counter for the satellites used in the position computation
*/
int usedInCalculations = 0;
/**
* approximate position of the receiver in ECEF.
*/
SimpleMatrix <Matrix> rxPosSimpleVector = Constellation.getRxPosAsVector(constellation.getRxPos());
if (firstExecution)
{
// Initialize the state vector
x_pred.set(idxX, rxPosSimpleVector.get(0));
x_pred.set(idxY, rxPosSimpleVector.get(1));
x_pred.set(idxZ, rxPosSimpleVector.get(2));
x_pred.set(idxClockBias, INITIAL_CLOCK_BIAS);
x_pred.set(idxClockDrift, 0.0); // clock bias drift
// Initialize the VCM of the state vector
P_pred.set(idxX, idxX, Math.Pow(INITIAL_SIGMAPOS, 2));
P_pred.set(idxY, idxY, Math.Pow(INITIAL_SIGMAPOS, 2));
P_pred.set(idxZ, idxZ, Math.Pow(INITIAL_SIGMAPOS, 2));
P_pred.set(idxClockBias, idxClockBias, Math.Pow(INITIAL_SIGMACLOCKBIAS, 2));
P_pred.set(idxClockDrift, idxClockDrift, Math.Pow(INITIAL_SIGMACLOCKDRIFT, 2));
x_pred.set(F.mult(x_pred));
P_pred = F.mult(P_pred.mult(F.transpose())).plus(Q);
}
else
{
// Perform time-prediction of the state vector and its VCM
x_pred.set(F.mult(x_meas));
P_pred = F.mult(P_meas.mult(F.transpose())).plus(Q);
}
/** Kalman gain matrix K
*/
SimpleMatrix <Matrix> K;
/** Innovation covariance
*/
SimpleMatrix <Matrix> S;
// Initialize the variables related to the measurement model
/** Observation Matrix H
*/
SimpleMatrix <Matrix> H = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, numStates);
/** pseudorange vector, one entry for every used satellite
*/
SimpleMatrix <Matrix> prVect = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 1); // pseurorange
/** predicted pseudoranges vector, one entry for every used satellite
*/
SimpleMatrix <Matrix> measPred = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 1); // pseudor. predicted
/** variance-covariance matrix of the measurements R
*/
SimpleMatrix <Matrix> R = SimpleMatrix <Matrix> .identity(CONSTELLATION_SIZE);
// R = R.divide(1.0/100.0);
// meas variance of each satellite
// SimpleMatrix<Matrix> sigma2C1 = new SimpleMatrix<Matrix>(CONSTELLATION_SIZE, 1);
double sigma2Meas = Math.Pow(5, 2);
// Form the observation matrix H
for (int k = 0; k < CONSTELLATION_SIZE; k++)
{
if (constellation.getSatellite(k).getSatellitePosition() == null)
{
continue;
}
// Get the raw pseudoranges for each satellite
prVect.set(k, constellation.getSatellite(k).getPseudorange());
// Compute the predicted (geometric) distance towards each satellite
distPred = Math.Sqrt(
Math.Pow(constellation.getSatellite(k).getSatellitePosition().getX()
- x_pred.get(idxX), 2)
+ Math.Pow(constellation.getSatellite(k).getSatellitePosition().getY()
- x_pred.get(idxY), 2)
+ Math.Pow(constellation.getSatellite(k).getSatellitePosition().getZ()
- x_pred.get(idxZ), 2)
);
// Set the values inside the H matrix
H.set(k, idxX, (x_pred.get(idxX) - constellation.getSatellite(k).getSatellitePosition().getX()) / distPred);
H.set(k, idxY, (x_pred.get(idxY) - constellation.getSatellite(k).getSatellitePosition().getY()) / distPred);
H.set(k, idxZ, (x_pred.get(idxZ) - constellation.getSatellite(k).getSatellitePosition().getZ()) / distPred);
H.set(k, idxClockBias, 1.0);
// Form the predicted measurement towards each satellite
measPred.set(k, distPred + x_pred.get(idxClockBias)
- constellation.getSatellite(k).getClockBias()
+ constellation.getSatellite(k).getAccumulatedCorrection());
// Form the VCM of the measurements (R)
elev = constellation.getSatellite(k).getRxTopo().getElevation() * (Math.PI / 180.0);
R.set(k, k, sigma2Meas * Math.Pow(a + b * Math.Exp(-elev / 10.0), 2));
usedInCalculations++;
}
if (usedInCalculations > 0)
{
// Compute the Kalman Gain. Catch exception if matrix inversion fails.
try
{
K = P_pred.mult(H.transpose().mult((H.mult(P_pred.mult(H.transpose())).plus(R)).invert()));
S = H.mult(P_pred.mult(H.transpose())).plus(R);
}
catch (Exception e)
{
Log.Error(NAME, new String(" Matrix inversion failed"), e);
return(Coordinates <Matrix> .globalXYZInstance(
rxPosSimpleVector.get(0),
rxPosSimpleVector.get(1),
rxPosSimpleVector.get(2)));
}
// Compute the Kalman innovation sequence
gamma = prVect.minus(measPred);
// Perform the measurement update
x_meas = x_pred.plus(K.mult(gamma));
P_meas = (SimpleMatrix <Matrix> .identity(numStates).minus((K.mult(H)))).mult(P_pred);
//// x_meas and P_meas are being used for the next set of measurements
//if (kalmanParamLogger.isStarted())
// kalmanParamLogger.logKalmanParam(x_meas, P_meas, numStates, gamma, S, CONSTELLATION_SIZE, constellation);
firstExecution = false;
return(Coordinates <Matrix> .globalXYZInstance(
x_meas.get(idxX),
x_meas.get(idxY),
x_meas.get(idxZ)));
}
else
{
return(Coordinates <Matrix> .globalXYZInstance(
rxPosSimpleVector.get(0),
rxPosSimpleVector.get(1),
rxPosSimpleVector.get(2)));
}
}
public double getE()
{
return(enu.get(0));
}
public double getX()
{
return(ecef.get(0));
}
public SatellitePosition computePositionGps(Observations obs, int satID, char satType, EphGps eph, double receiverClockError)
{
long unixTime = obs.getRefTime().getMsec();
double obsPseudorange = obs.getSatByIDType(satID, satType).getPseudorange(0);
// char satType2 = eph.getSatType() ;
if (satType != 'R')
{ // other than GLONASS
// System.out.println("### other than GLONASS data");
// Compute satellite clock error
double satelliteClockError = computeSatelliteClockError(unixTime, eph, obsPseudorange);
// Compute clock corrected transmission time
double tGPS = computeClockCorrectedTransmissionTime(unixTime, satelliteClockError, obsPseudorange);
// Compute eccentric anomaly
double Ek = computeEccentricAnomaly(tGPS, eph);
// Semi-major axis
double A = eph.getRootA() * eph.getRootA();
// Time from the ephemerides reference epoch
double tk = checkGpsTime(tGPS - eph.getToe());
// Position computation
double fk = Math.Atan2(Math.Sqrt(1 - Math.Pow(eph.getE(), 2))
* Math.Sin(Ek), Math.Cos(Ek) - eph.getE());
double phi = fk + eph.getOmega();
phi = Math.IEEERemainder(phi, 2 * Math.PI);
double u = phi + eph.getCuc() * Math.Cos(2 * phi) + eph.getCus()
* Math.Sin(2 * phi);
double r = A * (1 - eph.getE() * Math.Cos(Ek)) + eph.getCrc()
* Math.Cos(2 * phi) + eph.getCrs() * Math.Sin(2 * phi);
double ik = eph.getI0() + eph.getiDot() * tk + eph.getCic() * Math.Cos(2 * phi)
+ eph.getCis() * Math.Sin(2 * phi);
double Omega = eph.getOmega0()
+ (eph.getOmegaDot() - Constants.EARTH_ANGULAR_VELOCITY) * tk
- Constants.EARTH_ANGULAR_VELOCITY * eph.getToe();
Omega = Math.IEEERemainder(Omega + 2 * Math.PI, 2 * Math.PI);
double x1 = Math.Cos(u) * r;
double y1 = Math.Sin(u) * r;
// Coordinates
// double[][] data = new double[3][1];
// data[0][0] = x1 * Math.Cos(Omega) - y1 * Math.Cos(ik) * Math.Sin(Omega);
// data[1][0] = x1 * Math.Sin(Omega) + y1 * Math.Cos(ik) * Math.Cos(Omega);
// data[2][0] = y1 * Math.Sin(ik);
// Fill in the satellite position matrix
//this.coord.ecef = new SimpleMatrix<Matrix>(data);
//this.coord = Coordinates.globalXYZInstance(new SimpleMatrix<Matrix>(data));
SatellitePosition sp = new SatellitePosition(unixTime, satID, satType, x1 * Math.Cos(Omega) - y1 * Math.Cos(ik) * Math.Sin(Omega),
x1 * Math.Sin(Omega) + y1 * Math.Cos(ik) * Math.Cos(Omega),
y1 * Math.Sin(ik));
sp.setSatelliteClockError(satelliteClockError);
// Apply the correction due to the Earth rotation during signal travel time
SimpleMatrix <Matrix> R = computeEarthRotationCorrection(unixTime, receiverClockError, tGPS);
sp.setSMMultXYZ(R);
return(sp);
// this.setXYZ(x1 * Math.Cos(Omega) - y1 * Math.Cos(ik) * Math.Sin(Omega),
// x1 * Math.Sin(Omega) + y1 * Math.Cos(ik) * Math.Cos(Omega),
// y1 * Math.Sin(ik));
}
else
{ // GLONASS
// System.out.println("### GLONASS computation");
satID = eph.getSatID();
double X = eph.getX(); // satellite X coordinate at ephemeris reference time
double Y = eph.getY(); // satellite Y coordinate at ephemeris reference time
double Z = eph.getZ(); // satellite Z coordinate at ephemeris reference time
double Xv = eph.getXv(); // satellite velocity along X at ephemeris reference time
double Yv = eph.getYv(); // satellite velocity along Y at ephemeris reference time
double Zv = eph.getZv(); // satellite velocity along Z at ephemeris reference time
double Xa = eph.getXa(); // acceleration due to lunar-solar gravitational perturbation along X at ephemeris reference time
double Ya = eph.getYa(); // acceleration due to lunar-solar gravitational perturbation along Y at ephemeris reference time
double Za = eph.getZa(); // acceleration due to lunar-solar gravitational perturbation along Z at ephemeris reference time
/* NOTE: Xa,Ya,Za are considered constant within the integration interval (i.e. toe ?}15 minutes) */
double tn = eph.getTauN();
float gammaN = eph.getGammaN();
double tk = eph.gettk();
double En = eph.getEn();
double toc = eph.getToc();
double toe = eph.getToe();
int freqNum = eph.getfreq_num();
obs.getSatByIDType(satID, satType).setFreqNum(freqNum);
/*
* String refTime = eph.getRefTime().toString();
* // refTime = refTime.substring(0,10);
* refTime = refTime.substring(0,19);
* // refTime = refTime + " 00 00 00";
* System.out.println("refTime: " + refTime);
*
* try {
* // Set GMT time zone
* TimeZone zone = TimeZone.getTimeZone("GMT Time");
* // TimeZone zone = TimeZone.getTimeZone("UTC+4");
* DateFormat df = new java.text.SimpleDateFormat("yyyy MM dd HH mm ss");
* df.setTimeZone(zone);
*
* long ut = df.parse(refTime).getTime() ;
* System.out.println("ut: " + ut);
* Time tm = new Time(ut);
* double gpsTime = tm.getGpsTime();
* // double gpsTime = tm.getRoundedGpsTime();
* System.out.println("gpsT: " + gpsTime);
*
* } catch (ParseException e) {
* // TODO Auto-generated catch block
* e.printStackTrace();
* }
*/
// System.out.println("refTime: " + refTime);
// System.out.println("toc: " + toc);
// System.out.println("toe: " + toe);
// System.out.println("unixTime: " + unixTime);
// System.out.println("satID: " + satID);
// System.out.println("X: " + X);
// System.out.println("Y: " + Y);
// System.out.println("Z: " + Z);
// System.out.println("Xv: " + Xv);
// System.out.println("Yv: " + Yv);
// System.out.println("Zv: " + Zv);
// System.out.println("Xa: " + Xa);
// System.out.println("Ya: " + Ya);
// System.out.println("Za: " + Za);
// System.out.println("tn: " + tn);
// System.out.println("gammaN: " + gammaN);
//// System.out.println("tb: " + tb);
// System.out.println("tk: " + tk);
// System.out.println("En: " + En);
// System.out.println(" ");
/* integration step */
int int_step = 60; // [s]
/* Compute satellite clock error */
double satelliteClockError = computeSatelliteClockError(unixTime, eph, obsPseudorange);
// System.out.println("satelliteClockError: " + satelliteClockError);
/* Compute clock corrected transmission time */
double tGPS = computeClockCorrectedTransmissionTime(unixTime, satelliteClockError, obsPseudorange);
// System.out.println("tGPS: " + tGPS);
/* Time from the ephemerides reference epoch */
Time reftime = new Time(eph.getWeek(), tGPS);
double tk2 = checkGpsTime(tGPS - toe - reftime.getLeapSeconds());
// System.out.println("tk2: " + tk2);
/* number of iterations on "full" steps */
int n = (int)Math.Floor(Math.Abs(tk2 / int_step));
// System.out.println("Number of iterations: " + n);
/* array containing integration steps (same sign as tk) */
double[] array = new double[n];
Array.Fill(array, 1);
SimpleMatrix <Matrix> tkArray = new SimpleMatrix <Matrix>(n, 1, true, array);
// SimpleMatrix<Matrix> tkArray2 = tkArray.scale(2);
tkArray = tkArray.scale(int_step);
tkArray = tkArray.scale(tk2 / Math.Abs(tk2));
// tkArray.print();
//double ii = tkArray * int_step * (tk2/Math.Abs(tk2));
/* check residual iteration step (i.e. remaining fraction of int_step) */
double int_step_res = tk2 % int_step;
// System.out.println("int_step_res: " + int_step_res);
double[] intStepRes = new double[] { int_step_res };
SimpleMatrix <Matrix> int_stepArray = new SimpleMatrix <Matrix>(1, 1, false, intStepRes);
// int_stepArray.print();
/* adjust the total number of iterations and the array of iteration steps */
if (int_step_res != 0)
{
tkArray = tkArray.combine(n, 0, int_stepArray);
// tkArray.print();
n = n + 1;
// tkArray = [ii; int_step_res];
}
// System.out.println("n: " + n);
// numerical integration steps (i.e. re-calculation of satellite positions from toe to tk)
double[] pos = { X, Y, Z };
double[] vel = { Xv, Yv, Zv };
double[] acc = { Xa, Ya, Za };
double[] pos1;
double[] vel1;
SimpleMatrix <Matrix> posArray = new SimpleMatrix <Matrix>(1, 3, true, pos);
SimpleMatrix <Matrix> velArray = new SimpleMatrix <Matrix>(1, 3, true, vel);
SimpleMatrix <Matrix> accArray = new SimpleMatrix <Matrix>(1, 3, true, acc);
SimpleMatrix <Matrix> pos1Array;
SimpleMatrix <Matrix> vel1Array;
SimpleMatrix <Matrix> pos2Array;
SimpleMatrix <Matrix> vel2Array;
SimpleMatrix <Matrix> pos3Array;
SimpleMatrix <Matrix> vel3Array;
SimpleMatrix <Matrix> pos4Array;
SimpleMatrix <Matrix> vel4Array;
SimpleMatrix <Matrix> pos1dotArray;
SimpleMatrix <Matrix> vel1dotArray;
SimpleMatrix <Matrix> pos2dotArray;
SimpleMatrix <Matrix> vel2dotArray;
SimpleMatrix <Matrix> pos3dotArray;
SimpleMatrix <Matrix> vel3dotArray;
SimpleMatrix <Matrix> pos4dotArray;
SimpleMatrix <Matrix> vel4dotArray;
SimpleMatrix <Matrix> subPosArray;
SimpleMatrix <Matrix> subVelArray;
for (int i = 0; i < n; i++)
{
/* Runge-Kutta numerical integration algorithm */
// step 1
pos1Array = posArray;
//pos1 = pos;
vel1Array = velArray;
//vel1 = vel;
// differential position
pos1dotArray = velArray;
//double[] pos1_dot = vel;
vel1dotArray = satellite_motion_diff_eq(pos1Array, vel1Array, accArray, Constants.ELL_A_GLO, Constants.GM_GLO, Constants.J2_GLO, Constants.OMEGAE_DOT_GLO);
//double[] vel1_dot = satellite_motion_diff_eq(pos1, vel1, acc, Constants.ELL_A_GLO, Constants.GM_GLO, Constants.J2_GLO, Constants.OMEGAE_DOT_GLO);
// vel1dotArray.print();
// step 2
pos2Array = pos1dotArray.scale(tkArray.get(i)).divide(2);
pos2Array = posArray.plus(pos2Array);
//double[] pos2 = pos + pos1_dot*ii(i)/2;
// System.out.println("## pos2Array: " ); pos2Array.print();
vel2Array = vel1dotArray.scale(tkArray.get(i)).divide(2);
vel2Array = velArray.plus(vel2Array);
//double[] vel2 = vel + vel1_dot * tkArray.get(i)/2;
// System.out.println("## vel2Array: " ); vel2Array.print();
pos2dotArray = vel2Array;
//double[] pos2_dot = vel2;
vel2dotArray = satellite_motion_diff_eq(pos2Array, vel2Array, accArray, Constants.ELL_A_GLO, Constants.GM_GLO, Constants.J2_GLO, Constants.OMEGAE_DOT_GLO);
//double[] vel2_dot = satellite_motion_diff_eq(pos2, vel2, acc, Constants.ELL_A_GLO, Constants.GM_GLO, Constants.J2_GLO, Constants.OMEGAE_DOT_GLO);
// System.out.println("## vel2dotArray: " ); vel2dotArray.print();
// step 3
pos3Array = pos2dotArray.scale(tkArray.get(i)).divide(2);
pos3Array = posArray.plus(pos3Array);
// double[] pos3 = pos + pos2_dot * tkArray.get(i)/2;
// System.out.println("## pos3Array: " ); pos3Array.print();
vel3Array = vel2dotArray.scale(tkArray.get(i)).divide(2);
vel3Array = velArray.plus(vel3Array);
// double[] vel3 = vel + vel2_dot * tkArray.get(i)/2;
// System.out.println("## vel3Array: " ); vel3Array.print();
pos3dotArray = vel3Array;
//double[] pos3_dot = vel3;
vel3dotArray = satellite_motion_diff_eq(pos3Array, vel3Array, accArray, Constants.ELL_A_GLO, Constants.GM_GLO, Constants.J2_GLO, Constants.OMEGAE_DOT_GLO);
//double[] vel3_dot = satellite_motion_diff_eq(pos3, vel3, acc, Constants.ELL_A_GLO, Constants.GM_GLO, Constants.J2_GLO, Constants.OMEGAE_DOT_GLO);
// System.out.println("## vel3dotArray: " ); vel3dotArray.print();
// step 4
pos4Array = pos3dotArray.scale(tkArray.get(i));
pos4Array = posArray.plus(pos4Array);
//double[] pos4 = pos + pos3_dot * tkArray.get(i);
// System.out.println("## pos4Array: " ); pos4Array.print();
vel4Array = vel3dotArray.scale(tkArray.get(i));
vel4Array = velArray.plus(vel4Array);
//double[] vel4 = vel + vel3_dot * tkArray.get(i);
// System.out.println("## vel4Array: " ); vel4Array.print();
pos4dotArray = vel4Array;
//double[] pos4_dot = vel4;
vel4dotArray = satellite_motion_diff_eq(pos4Array, vel4Array, accArray, Constants.ELL_A_GLO, Constants.GM_GLO, Constants.J2_GLO, Constants.OMEGAE_DOT_GLO);
//double[] vel4_dot = satellite_motion_diff_eq(pos4, vel4, acc, Constants.ELL_A_GLO, Constants.GM_GLO, Constants.J2_GLO, Constants.OMEGAE_DOT_GLO);
// System.out.println("## vel4dotArray: " ); vel4dotArray.print();
// final position and velocity
subPosArray = pos1dotArray.plus(pos2dotArray.scale(2)).plus(pos3dotArray.scale(2)).plus(pos4dotArray);
subPosArray = subPosArray.scale(tkArray.get(i)).divide(6);
posArray = posArray.plus(subPosArray);
//pos = pos + (pos1_dot + 2*pos2_dot + 2*pos3_dot + pos4_dot)*ii(s)/6;
// System.out.println("## posArray: " ); posArray.print();
subVelArray = vel1dotArray.plus(vel2dotArray.scale(2)).plus(vel3dotArray.scale(2)).plus(vel4dotArray);
subVelArray = subVelArray.scale(tkArray.get(i)).divide(6);
velArray = velArray.plus(subVelArray);
//vel = vel + (vel1_dot + 2*vel2_dot + 2*vel3_dot + vel4_dot)*ii(s)/6;
// System.out.println("## velArray: " ); velArray.print();
// System.out.println(" " );
}
/* transformation from PZ-90.02 to WGS-84 (G1150) */
double x1 = posArray.get(0) - 0.36;
double y1 = posArray.get(1) + 0.08;
double z1 = posArray.get(2) + 0.18;
/* satellite velocity */
double Xv1 = velArray.get(0);
double Yv1 = velArray.get(1);
double Zv1 = velArray.get(2);
/* Fill in the satellite position matrix */
SatellitePosition sp = new SatellitePosition(unixTime, satID, satType, x1, y1, z1);
sp.setSatelliteClockError(satelliteClockError);
//
// /* Apply the correction due to the Earth rotation during signal travel time */
SimpleMatrix <Matrix> R = computeEarthRotationCorrection(unixTime, receiverClockError, tGPS);
sp.setSMMultXYZ(R);
return(sp);
// return null ;
}
}