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 ;
}
}
