public SatellitePosition getSatPositionAndVelocities(long unixTime, double range, int satID, char satType, double receiverClockError)
{
//long unixTime = obs.getRefTime().getMsec();
//double range = obs.getSatByIDType(satID, satType).getPseudorange(0);
if (range == 0)
{
return(null);
}
EphGps eph = findEph(unixTime, satID, satType);
if (eph == null)
{
Log.Error(TAG, "getSatPositionAndVelocities: Ephemeris failed to load...");
return(null);
}
if (eph.Equals(EphGps.UnhealthyEph))
{
return(SatellitePosition.UnhealthySat);
}
// char satType = eph.getSatType();
SatellitePosition sp = computeSatPositionAndVelocities(unixTime, range, satID, satType, eph, receiverClockError);
// SatellitePosition sp = computePositionGps(unixTime, satType, satID, eph, range, receiverClockError);
//if(receiverPosition!=null) earthRotationCorrection(receiverPosition, sp);
return(sp);// new SatellitePosition(eph, unixTime, satID, range);
}
/*
* public RinexNavigationParserGps(EphemerisResponse ephResponse)
* {
* foreach (GnssEphemeris eph in ephResponse.ephList)
* {
* if (eph is GpsEphemeris)
* {
* this.eph.Add(new EphGps((GpsEphemeris)eph));
* }
* }
* this.iono = new IonoGps(ephResponse.ionoProto);
* }
*/
public SatellitePosition getGpsSatPosition(Observations obs, int satID, char satType, double receiverClockError)
{
long unixTime = obs.getRefTime().getMsec();
double range = obs.getSatByIDType(satID, satType).getPseudorange(0);
if (range == 0)
{
return(null);
}
EphGps eph = findEph(unixTime, satID, satType);
if (eph.Equals(EphGps.UnhealthyEph))
{
return(SatellitePosition.UnhealthySat);
}
if (eph != null)
{
// char satType = eph.getSatType();
SatellitePosition sp = computePositionGps(obs, satID, satType, eph, receiverClockError);
// SatellitePosition sp = computePositionGps(unixTime, satType, satID, eph, range, receiverClockError);
//if(receiverPosition!=null) earthRotationCorrection(receiverPosition, sp);
return(sp);// new SatellitePosition(eph, unixTime, satID, range);
}
return(null);
}
void Start()
{
if (satellitePosition == null)
{
satellitePosition = new SatellitePosition(gameObject);
satellitePosition.GenerateSeed();
}
else
{
satellitePosition.SetParent(gameObject);
}
if (Data == null)
{
return;
}
if (Data.IsEmpty())
{
Create();
}
else
{
Visualize();
}
}
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 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));
}
public override void calculateSatPosition(Location initialLocation, Coordinates <Matrix> position)
{
// Make a list to hold the satellites that are to be excluded based on elevation/CN0 masking criteria
List <SatelliteParameters> excludedSatellites = new List <SatelliteParameters>();
lock (this) {
rxPos = Coordinates <Matrix> .globalXYZInstance(position.getX(), position.getY(), position.getZ());
foreach (SatelliteParameters observedSatellite in observedSatellites)
{
/*
* Computation of the Galileo satellite coordinates in ECEF frame
*/
// Determine the current Galileo week number (info: is the same as GPS week number)
// todo: confirm difference to github fork
int galileoWeek = (int)weekNumber;
// Time of signal reception in Galileo Seconds of the Week (SoW)
double galileoSow = (tRxGalileoTOW) * 1e-9;
Time tGalileo = new Time(galileoWeek, galileoSow);
// Convert the time of reception from GPS SoW to UNIX time (milliseconds)
long timeRx = tGalileo.getMsec();
/*Compute the Galileo satellite coordinates
*
* INPUT:
* @param timeRx = time of measurement reception - UNIX [milliseconds]
* @param pseudorange = pseudorange measuremnent [meters]
* @param satID = satellite ID
* @param satType = satellite type indicating the constellation (E: Galileo)
*
*/
SatellitePosition rnp = ((RinexNavigationGalileo)rinexNavGalileo).getGalileoSatPosition(
timeRx,
observedSatellite.getPseudorange(),
observedSatellite.getSatId(),
satType,
0.0,
initialLocation);
if (rnp == null)
{
excludedSatellites.Add(observedSatellite);
GnssCoreService.notifyUser("Failed getting ephemeris data!", Snackbar.LengthShort, RNP_NULL_MESSAGE);
continue;
}
//observedSatellite.setSatellitePosition(rnp);
/* Compute the azimuth and elevation w.r.t the user's approximate location
*
* INPUT:
* @param rxPos = user's approximate ECEF coordinates [cartesian]
* @param satellitePosition = satellite ECEF coordinates [cartesian]
*
*/
observedSatellite.setRxTopo(
new TopocentricCoordinates <Matrix>(
rxPos,
observedSatellite.getSatellitePosition()));
// Add to the exclusion list the satellites that do not pass the masking criteria
if (observedSatellite.getRxTopo().getElevation() < MASK_ELEVATION)
{
excludedSatellites.Add(observedSatellite);
continue;
}
// Initialize the variable to hold the results of the entire pseudorange correction models
double accumulatedCorrection = 0;
/* Compute the accumulated corrections for the pseudorange measurements
* Currently the accumulated corrections contain the following effects:
* - Ionosphere
* - Troposphere
* - Shapiro delay (i.e, relativistic path range correction)
*
* INPUT:
* @param timeRx = time of measurement reception - UNIX [milliseconds]
* @param rxPos = user's approximate ECEF coordinates [cartesian]
* @param satellitePosition = satellite ECEF coordinates [cartesian]
* @param rinexNavGalileo = RinexNavigationGalileo type object
*/
//foreach (Correction correction in corrections)
//{
// correction.calculateCorrection(
// new Time(timeRx),
// rxPos,
// observedSatellite.getSatellitePosition(),
// rinexNavGalileo,
// initialLocation);
// accumulatedCorrection += correction.getCorrection();
//}
observedSatellite.setAccumulatedCorrection(accumulatedCorrection);
}
// Remove from the list all the satellites that did not pass the masking criteria
visibleButNotUsed += excludedSatellites.Count();
//observedSatellites.removeAll(excludedSatellites);
unusedSatellites.AddRange(excludedSatellites);
}
}
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 ;
}
}
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;
}
void Start()
{
SolForce = 0;
EarthForce = 0;
//GetComponent<Rigidbody>().mass = SpaceMath.SolMass * SpaceMath.ToEngineMass;
//Earth.GetComponent<Rigidbody>().mass = SpaceMath.EarthMass * SpaceMath.ToEngineMass;
//Sat.GetComponent<Rigidbody>().mass = 7.34f * Mathf.Pow(10f, 22) * SpaceMath.ToEngineMass;
//float jupMass = 1.8982f * Mathf.Pow(10, 27);
//Debug.Log(70f * jupMass);
//Debug.Log(4f * Mathf.Pow(10, -27));
//Debug.Log((1.304f * Mathf.Pow(10f, 36f)) * SpaceMath.ToEngineMass );
//Debug.Log(4.18f * Mathf.Pow(70000000f, 3) * 1700);
//Debug.Log(4.18f * Mathf.Pow(10.5f * SpaceMath.EarthRadius, 3) * 1326);
//Debug.Log(4.18f * Mathf.Pow(250000000, 3) * 1700);
//Debug.Log(4.18f * Mathf.Pow(50000000f, 3) * 10000f);
//Debug.Log(4.18f * Mathf.Pow(64400000f, 3) * 100000f);
//Debug.Log((1.11f * Mathf.Pow(10, 28)) / SpaceMath.SolMass);
//Debug.Log(0.079f * SpaceMath.SolMass );
//Debug.Log( (1.89 * Mathf.Pow(10, 27)) / SpaceMath.SolMass );
//Debug.Log(Mathf.Pow( (70f * jupMass) / (4.18f * 100000f), 1f/3f));
//6957000
//64400000
//140000000
//147000000
//255000000 70 Jup
//275000000 min star
//Debug.Log(Mathf.Pow((0.075f * SpaceMath.SolMass) / (4.18f * 1700f), 1f / 3f));
//Debug.Log(64400000f / SpaceMath.SolRadius);
//Debug.Log(0.01f * SpaceMath.SolRadius);
//Debug.Log((70f * jupMass) / SpaceMath.SolMass);
//Debug.Log(Mathf.Pow((0.079f * SpaceMath.SolMass) / (4.18f * 1700), 1f / 3f) / SpaceMath.SolRadius);
//Debug.Log((0.079f * SpaceMath.SolMass) / (4.18f * Mathf.Pow(0.099f * SpaceMath.SolRadius, 3)));
SolForce = SpaceMath.Gravity.GetGravityForce(gameObject, Sat);
EarthForce = SpaceMath.Gravity.GetGravityForce(Earth, Sat);
//Debug.Log(this.GetComponent<Rigidbody>().mass / Mathf.Pow( (Sat.transform.position - this.transform.position).magnitude, 2));
//Debug.Log(Earth.GetComponent<Rigidbody>().mass / Mathf.Pow( (Sat.transform.position - Earth.transform.position).magnitude, 2));
//Debug.Log(SpaceMath.SolRadius * 109.25f);
//Debug.Log((SpaceMath.SolRadius * 109.25f) / SpaceMath.AU);
//Debug.Log( 50f * SpaceMath.AU);
//PlanetPosition planetPosition = new PlanetPosition();
//planetPosition.GenerateSeed();
//for (int i = 0; i < 10; i++)
//{
// Debug.Log(planetPosition.PositionAt(i));
//}
SatellitePosition satellitePosition = new SatellitePosition();
satellitePosition.GenerateSeed();
for (int i = 0; i < 10; i++)
{
Debug.Log(satellitePosition.PositionAt(i));
}
//for (int i = 0; i < 10; i++)
//{
// float d = 9.1f * Mathf.Pow(0.52f, i);
// //float d = 0.5f * Mathf.Pow(9.52f, i);
// Debug.Log((d + 4) / 10);
// //Debug.Log(((d + 4) / 10) * SpaceMath.AU);
// //Debug.Log(pos.NextPosition());
//}
//Debug.Log((4.18 * 46656) / (6 * Mathf.Pow(10, 16)));
}
override public void calculateSatPosition(Location initialLocation, Coordinates <Matrix> position)
{
//// Make a list to hold the satellites that are to be excluded based on elevation/CN0 masking criteria
List <SatelliteParameters> excludedSatellites = new List <SatelliteParameters>();
lock (this) {
rxPos = Coordinates <Matrix> .globalXYZInstance(position.getX(), position.getY(), position.getZ());
foreach (SatelliteParameters observedSatellite in observedSatellites)
{
// Computation of the GPS satellite coordinates in ECEF frame
// Determine the current GPS week number
int gpsWeek = (int)(weekNumberNanos / Constants.NUMBER_NANO_SECONDS_PER_WEEK);
// Time of signal reception in GPS Seconds of the Week (SoW)
double gpsSow = (tRxGPS - weekNumberNanos) * 1e-9;
Time tGPS = new Time(gpsWeek, gpsSow);
// Convert the time of reception from GPS SoW to UNIX time (milliseconds)
long timeRx = tGPS.getMsec();
SatellitePosition rnp = ((RinexNavigationGps)rinexNavGps).getSatPositionAndVelocities(
timeRx,
observedSatellite.getPseudorange(),
observedSatellite.getSatId(),
satType,
0.0,
initialLocation);
if (rnp == null)
{
excludedSatellites.Add(observedSatellite);
//GnssCoreService.notifyUser("Failed getting ephemeris data!", Snackbar.LengthShort, RNP_NULL_MESSAGE);
continue;
}
observedSatellite.setSatellitePosition(rnp);
observedSatellite.setRxTopo(
new TopocentricCoordinates <Matrix>(
rxPos,
observedSatellite.getSatellitePosition()));
// // Add to the exclusion list the satellites that do not pass the masking criteria
if (observedSatellite.getRxTopo().getElevation() < MASK_ELEVATION)
{
excludedSatellites.Add(observedSatellite);
}
double accumulatedCorrection = 0;
foreach (Correction correction in corrections)
{
correction.calculateCorrection(
new Java.Sql.Time(timeRx),
rxPos,
observedSatellite.getSatellitePosition(),
rinexNavGps,
initialLocation);
accumulatedCorrection += correction.getCorrection();
}
observedSatellite.setAccumulatedCorrection(accumulatedCorrection);
}
// Remove from the list all the satellites that did not pass the masking criteria
visibleButNotUsed += excludedSatellites.Count();
foreach (SatelliteParameters sp in excludedSatellites)
{
observedSatellites.Remove(sp);
}
unusedSatellites.AddRange(excludedSatellites);
}
}
