// BDS is different in that some satellites are in GEO orbits.
// According to the ICD, the
// SV position derivation for MEO and IGSO is identical to
// that for other kepler+perturbation systems (e.g. GPS); however,
// the position derivation for the GEO SVs is different.
// According to the ICD, the GEO SVs are those with PRNs 1-5.
// The following method overrides OrbitEph.svXvt( ). It uses
// OrbitEph::svXvt( ) for PRNs above 5, but implements a different
// algorithm for PRNs 1-5.
Xvt BDSEphemeris::svXvt(const CommonTime& t) const
{
if(!dataLoadedFlag)
GPSTK_THROW(InvalidRequest("Data not loaded"));
// If the PRN ID is greatet than 5, assume this
// is a MEO or IGSO SV and use the standard OrbitEph
// version of svXvt
if (satID.id>5) return(OrbitEph::svXvt(t));
// If PRN ID is in the range 1-5, treat this as a GEO
//
// The initial calculations are identical to the standard
// Kepler+preturbation model
Xvt sv;
double ea; // eccentric anomaly
double delea; // delta eccentric anomaly during iteration
double elapte; // elapsed time since Toe
//double elaptc; // elapsed time since Toc
//double dtc,dtr;
double q,sinea,cosea;
double GSTA,GCTA;
double amm;
double meana; // mean anomaly
double F,G; // temporary real variables
double alat,talat,c2al,s2al,du,dr,di,U,R,truea,AINC;
double ANLON,cosu,sinu,xip,yip,can,san,cinc,sinc;
double dek,dlk,div,duv,drv;
double dxp,dyp;
double xGK,yGK,zGK;
WGS84Ellipsoid ell;
double sqrtgm = SQRT(ell.gm());
double twoPI = 2.0e0 * PI;
double lecc; // eccentricity
double tdrinc; // dt inclination
double Ahalf = SQRT(A); // A is semi-major axis of orbit
double ToeSOW = GPSWeekSecond(ctToe).sow; // SOW is time-system-independent
lecc = ecc;
tdrinc = idot;
// Compute time since ephemeris & clock epochs
elapte = t - ctToe;
// Compute mean motion
amm = (sqrtgm / (A*Ahalf)) + dn;
// In-plane angles
// meana - Mean anomaly
// ea - Eccentric anomaly
// truea - True anomaly
meana = M0 + elapte * amm;
meana = fmod(meana, twoPI);
ea = meana + lecc * ::sin(meana);
int loop_cnt = 1;
do {
F = meana - (ea - lecc * ::sin(ea));
G = 1.0 - lecc * ::cos(ea);
delea = F/G;
ea = ea + delea;
loop_cnt++;
} while ((fabs(delea) > 1.0e-11) && (loop_cnt <= 20));
// Compute clock corrections
sv.relcorr = svRelativity(t);
sv.clkbias = svClockBias(t);
sv.clkdrift = svClockDrift(t);
sv.frame = ReferenceFrame::WGS84;
// Compute true anomaly
q = SQRT(1.0e0 - lecc*lecc);
sinea = ::sin(ea);
cosea = ::cos(ea);
G = 1.0e0 - lecc * cosea;
// G*SIN(TA) AND G*COS(TA)
GSTA = q * sinea;
GCTA = cosea - lecc;
// True anomaly
truea = atan2 (GSTA, GCTA);
// Argument of lat and correction terms (2nd harmonic)
alat = truea + w;
talat = 2.0e0 * alat;
c2al = ::cos(talat);
s2al = ::sin(talat);
du = c2al * Cuc + s2al * Cus;
dr = c2al * Crc + s2al * Crs;
di = c2al * Cic + s2al * Cis;
// U = updated argument of lat, R = radius, AINC = inclination
U = alat + du;
R = A*G + dr;
AINC = i0 + tdrinc * elapte + di;
// At this point, the ICD formulation diverges to something
// different.
// Longitude of ascending node (ANLON)
ANLON = OMEGA0 + OMEGAdot * elapte
- ell.angVelocity() * ToeSOW;
// In plane location
cosu = ::cos(U);
sinu = ::sin(U);
xip = R * cosu;
yip = R * sinu;
// Angles for rotation
can = ::cos(ANLON);
san = ::sin(ANLON);
cinc = ::cos(AINC);
sinc = ::sin(AINC);
// GEO satellite coordinates in user-defined inertial system
xGK = xip*can - yip*cinc*san;
yGK = xip*san + yip*cinc*can;
zGK = yip*sinc;
// Rz matrix
double angleZ = ell.angVelocity() * elapte;
double cosZ = ::cos(angleZ);
double sinZ = ::sin(angleZ);
// Initiailize 3X3 with all 0.0
gpstk::Matrix<double> matZ(3,3);
// Row,Col
matZ(0,0) = cosZ;
matZ(0,1) = sinZ;
matZ(0,2) = 0.0;
matZ(1,0) = -sinZ;
matZ(1,1) = cosZ;
matZ(1,2) = 0.0;
matZ(2,0) = 0.0;
matZ(2,1) = 0.0;
matZ(2,2) = 1.0;
// Rx matrix
double angleX = -5.0 * PI/180.0; /// This is a constant. Should set it once
double cosX = ::cos(angleX);
double sinX = ::sin(angleX);
gpstk::Matrix<double> matX(3,3);
matX(0,0) = 1.0;
matX(0,1) = 0.0;
matX(0,2) = 0.0;
matX(1,0) = 0.0;
matX(1,1) = cosX;
matX(1,2) = sinX;
matX(2,0) = 0.0;
matX(2,1) = -sinX;
matX(2,2) = cosX;
// Matrix (single column) of xGK, yGK, zGK
gpstk::Matrix<double> inertialPos(3,1);
inertialPos(0,0) = xGK;
inertialPos(1,0) = yGK;
inertialPos(2,0) = zGK;
gpstk::Matrix<double> result(3,1);
result = matZ * matX * inertialPos;
sv.x[0] = result(0,0);
sv.x[1] = result(1,0);
sv.x[2] = result(2,0);
// derivatives of true anamoly and arg of latitude
dek = amm / G;
dlk = Ahalf * q * sqrtgm / (R*R);
// in-plane, cross-plane, and radial derivatives
div = tdrinc - 2.0e0 * dlk * (Cis * c2al - Cic * s2al);
duv = dlk*(1.e0+ 2.e0 * (Cus*c2al - Cuc*s2al));
drv = A * lecc * dek * sinea + 2.e0 * dlk * (Crs * c2al - Crc * s2al);
//
dxp = drv*cosu - R*sinu*duv;
dyp = drv*sinu + R*cosu*duv;
// Time-derivative of Rz matrix
gpstk::Matrix<double> dmatZ(3,3);
// Row,Col
dmatZ(0,0) = sinZ * -ell.angVelocity();
dmatZ(0,1) = -cosZ * -ell.angVelocity();
dmatZ(0,2) = 0.0;
dmatZ(1,0) = cosZ * -ell.angVelocity();
dmatZ(1,1) = sinZ * -ell.angVelocity();
dmatZ(1,2) = 0.0;
dmatZ(2,0) = 0.0;
dmatZ(2,1) = 0.0;
dmatZ(2,2) = 0.0;
// Time-derivative of X,Y,Z in interial form
gpstk::Matrix<double> dIntPos(3,1);
dIntPos(0,0) = - xip * san * OMEGAdot
+ dxp * can
- yip * (cinc * can * OMEGAdot
-sinc * san * div )
- dyp * cinc * san;
dIntPos(1,0) = xip * can * OMEGAdot
+ dxp * san
- yip * (cinc * san * OMEGAdot
+sinc * can * div )
+ dyp * cinc * can;
dIntPos(2,0) = yip * cinc * div + dyp * sinc;
/*
cout << "dIntPos : " << dIntPos(0,0)
<< ", " << dIntPos(1,0)
<< ", " << dIntPos(2,0) << endl;
double vMag = ::sqrt(dIntPos(0,0)*dIntPos(0,0) +
dIntPos(1,0)*dIntPos(1,0) +
dIntPos(2,0)*dIntPos(2,0) );
cout << " dIntPos Mag: " << vMag << endl;
cout << "dmatZ : " << dmatZ(0,0)
<< ", " << dmatZ(0,1)
<< ", " << dmatZ(0,2) << endl;
cout << "dmatZ : " << dmatZ(1,0)
<< ", " << dmatZ(1,1)
<< ", " << dmatZ(1,2) << endl;
cout << "dmatZ : " << dmatZ(2,0)
<< ", " << dmatZ(2,1)
<< ", " << dmatZ(2,2) << endl;
*/
gpstk::Matrix<double> vresult(3,1);
vresult = matZ * matX * dIntPos +
dmatZ * matX * inertialPos;
/* debug
gpstk::Matrix<double> firstHalf(3,1);
firstHalf = matZ * matX * dIntPos;
gpstk::Matrix<double> secondHalf(3,1);
secondHalf = dmatZ * matX * inertialPos;
cout << "firstHalf: " << firstHalf(0,0)
<< ", " << firstHalf(1,0)
<< ", " << firstHalf(2,0) << endl;
cout << "secondHalf: " << secondHalf(0,0)
<< ", " << secondHalf(1,0)
<< ", " << secondHalf(2,0) << endl;
end debug */
// Move results into output variables
sv.v[0] = vresult(0,0);
sv.v[1] = vresult(1,0);
sv.v[2] = vresult(2,0);
return sv;
}
/*--------------------------------------------------------------------*/
void cppinit2(int *idproc, int *nvp, int argc, char *argv[]) {
/* this subroutine initializes parallel processing
lgrp communicator = MPI_COMM_WORLD
output: idproc, nvp
idproc = processor id in lgrp communicator
nvp = number of real or virtual processors obtained
local data */
static int ibig = 2147483647;
static float small = 1.0e-12;
int ierror, flag, ndprec, idprec, iprec;
float prec;
prec = 1.0 + small;
iprec = ibig + 1;
/* ndprec = (0,1) = (no,yes) use (normal,autodouble) precision */
if (vresult(prec) > 1.0)
ndprec = 1;
else
ndprec = 0;
/* idprec = (0,1) = (no,yes) use (normal,autodouble) integer precision */
if (iresult(iprec) > 0)
idprec = 1;
else
idprec = 0;
/* Open error file */
unit2 = fopen("C.2","w");
/* indicate whether MPI_INIT has been called */
ierror = MPI_Initialized(&flag);
if (!flag) {
/* initialize the MPI execution environment */
ierror = MPI_Init(&argc,&argv);
if (ierror) exit(1);
}
lworld = MPI_COMM_WORLD;
lgrp = lworld;
/* determine the rank of the calling process in the communicator */
ierror = MPI_Comm_rank(lgrp,idproc);
/* determine the size of the group associated with a communicator */
ierror = MPI_Comm_size(lgrp,&nproc);
/* set default datatypes */
mint = MPI_INT;
mdouble = MPI_DOUBLE;
/* single precision real */
if (ndprec==0) {
mreal = MPI_FLOAT;
mcplx = MPI_COMPLEX;
}
/* double precision real */
else {
mreal = MPI_DOUBLE;
mcplx = MPI_DOUBLE_COMPLEX;
}
/* single precision integer */
/* if (idprec==0) */
/* mint = MPI_INT; */
/* double precision integer */
/* else */
/* mint = MPI_LONG; */
/* operators */
msum = MPI_SUM;
mmax = MPI_MAX;
*nvp = nproc;
return;
}