void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
//The Julan date in TT at the epoch used in the orbital propagation
//algorithm: 0:00 January 1 1950
const double epochDate=2433281.5;
double *deltaT;
size_t numTimes;
double SGP4Elements[7],elementEpochTime;
bool opsMode=0;
char opsChar;
bool gravityModel=0;
gravconsttype gravConstType;
mxArray *retMat;
double *retVec;
mxArray *errorMat;
double *errorVals;
if(nrhs<2||nrhs>6||nrhs==3) {
mexErrMsgTxt("Incorrect number of inputs.");
return;
}
if(nlhs>2) {
mexErrMsgTxt("Wrong number of outputs.");
return;
}
{
const size_t M=mxGetM(prhs[0]);
const size_t N=mxGetN(prhs[0]);
if(!((M==7&&N==1)||(N==7&&M==1))) {
mexErrMsgTxt("The SGP4 elements have the wrong dimensionality.");
return;
}
}
checkRealDoubleArray(prhs[0]);
{
size_t i;
double *theEls;
theEls=(double*)mxGetData(prhs[0]);
//Copy the elements into SGP4Elements, adjusting the units from SI
//to the values used here.
for(i=0;i<5;i++) {
SGP4Elements[i]=theEls[i];
}
//The multipliation by 60 changes the value from radians per second
//to radians per minute as desired by the SGP4 code.
SGP4Elements[5]=theEls[5]*60.0;
SGP4Elements[6]=theEls[6];
}
{
const size_t M=mxGetM(prhs[1]);
const size_t N=mxGetN(prhs[1]);
if((M!=1&&N!=1)||mxIsEmpty(prhs[1])) {
mexErrMsgTxt("The times have the wrong dimensionality.");
return;
}
numTimes=std::max(M,N);
}
checkRealDoubleArray(prhs[1]);
deltaT=(double*)mxGetData(prhs[1]);
if(nrhs>2&&!mxIsEmpty(prhs[2])&&!mxIsEmpty(prhs[3])) {
const double TT1=getDoubleFromMatlab(prhs[2]);
const double TT2=getDoubleFromMatlab(prhs[3]);
if(TT1>TT2) {
elementEpochTime=(TT1-epochDate)+TT2;
} else {
elementEpochTime=(TT2-epochDate)+TT1;
}
} else {
//No time is given. Make sure that the deep space propagator is not
//going to be used. Otherwise, the time value does not matter.
elementEpochTime=0;
if(2*pi/SGP4Elements[5]>=225) {
mexErrMsgTxt("The elements imply the use of the deep space propagator, but no time was given.");
return;
}
}
if(nrhs>4) {
opsMode=getBoolFromMatlab(prhs[4]);
}
if(opsMode==0) {
opsChar='a';
} else {
opsChar='i';
}
if(nrhs>5) {
gravityModel=getBoolFromMatlab(prhs[5]);
}
if(gravityModel==0) {
gravConstType=wgs72;
} else {
gravConstType=wgs84;
}
//Allocate space for the return values
retMat=mxCreateDoubleMatrix(6,numTimes,mxREAL);
retVec=(double*)mxGetData(retMat);
errorMat=mxCreateDoubleMatrix(numTimes,1,mxREAL);
errorVals=(double*)mxGetData(errorMat);
{
//This will be filled with the ephemeris information needed for
//propagating the satellite.
elsetrec satRec;
size_t i;
sgp4init(gravConstType,//Specified WGS-84 or WGS-72
opsChar,
elementEpochTime,//Epoch time of the orbital elements.
SGP4Elements[6],//BSTAR drag term.
SGP4Elements[0],//Eccentricity
SGP4Elements[2],//Argument of perigee
SGP4Elements[1],//Inclination
SGP4Elements[4],//Mean anomaly
SGP4Elements[5],//Mean motion
SGP4Elements[3],//Righ ascension of the ascending node.
satRec);//Gets filled by the function.
//Do the actual propagation.
//The division by 60 transforms the time value from seconds to
//minutes, as the SGP4 code wants the result in minutes.
for(i=0;i<numTimes;i++) {
double *r=retVec+6*i;
double *v=retVec+6*i+3;
int j;
sgp4(gravConstType, satRec, deltaT[i]/60.0, r, v);
//Convert the units from kilometers and kilometers per second
//to meters and meters per second.
for(j=0;j<3;j++) {
r[j]=1000*r[j];
v[j]=1000*v[j];
}
errorVals[i]=(double)satRec.error;
}
}
//Save the result
plhs[0]=retMat;
//If the error value is desired.
if(nlhs>1) {
plhs[1]=errorMat;
} else{
mxDestroyArray(errorMat);
}
}
void twoline2rv
(
char longstr1[130], char longstr2[130],
char typerun, char typeinput, char opsmode,
gravconsttype whichconst,
double& startmfe, double& stopmfe, double& deltamin,
sgp4struct &satrec
)
{
const double deg2rad = pi / 180.0; // 0.0174532925199433
const double xpdotp = 1440.0 / (2.0 * pi); // 229.1831180523293
double sec, mu, radiusearthkm, tumin, xke, j2, j3, j4, j3oj2;
double startsec, stopsec, startdayofyr, stopdayofyr, jdstart, jdstop;
int startyear, stopyear, startmon, stopmon, startday, stopday,
starthr, stophr, startmin, stopmin;
int cardnumb, numb, j;
long revnum = 0, elnum = 0;
char classification, intldesg[11];
int year = 0;
int mon, day, hr, minute, nexp, ibexp;
getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 );
satrec.error = 0;
// set the implied decimal points since doing a formated read
// fixes for bad input data values (missing, ...)
for (j = 10; j <= 15; j++)
if (longstr1[j] == ' ')
longstr1[j] = '_';
if (longstr1[44] != ' ')
longstr1[43] = longstr1[44];
longstr1[44] = '.';
if (longstr1[7] == ' ')
longstr1[7] = 'U';
if (longstr1[9] == ' ')
longstr1[9] = '.';
for (j = 45; j <= 49; j++)
if (longstr1[j] == ' ')
longstr1[j] = '0';
if (longstr1[51] == ' ')
longstr1[51] = '0';
if (longstr1[53] != ' ')
longstr1[52] = longstr1[53];
longstr1[53] = '.';
longstr2[25] = '.';
for (j = 26; j <= 32; j++)
if (longstr2[j] == ' ')
longstr2[j] = '0';
if (longstr1[62] == ' ')
longstr1[62] = '0';
if (longstr1[68] == ' ')
longstr1[68] = '0';
sscanf(longstr1,"%2d %5ld %1c %10s %2d %12lf %11lf %7lf %2d %7lf %2d %2d %6ld ",
&cardnumb,&satrec.noradNum,&classification, intldesg, &satrec.epochyr,
&satrec.epochdays,&satrec.ndot, &satrec.nddot, &nexp, &satrec.bstar,
&ibexp, &numb, &elnum );
if (typerun == 'v') // run for specified times from the file
{
if (longstr2[52] == ' ')
sscanf(longstr2,"%2d %5ld %9lf %9lf %8lf %9lf %9lf %10lf %6ld %lf %lf %lf \n",
&cardnumb,&satrec.noradNum, &satrec.inclination,
&satrec.rightAscNode,&satrec.eccentricity, &satrec.argpo, &satrec.meanAnomaly, &satrec.meanMotion,
&revnum, &startmfe, &stopmfe, &deltamin );
else
sscanf(longstr2,"%2d %5ld %9lf %9lf %8lf %9lf %9lf %11lf %6ld %lf %lf %lf \n",
&cardnumb,&satrec.noradNum, &satrec.inclination,
&satrec.rightAscNode,&satrec.eccentricity, &satrec.argpo, &satrec.meanAnomaly, &satrec.meanMotion,
&revnum, &startmfe, &stopmfe, &deltamin );
}
else // simply run -1 day to +1 day or user input times
{
if (longstr2[52] == ' ')
sscanf(longstr2,"%2d %5ld %9lf %9lf %8lf %9lf %9lf %10lf %6ld \n",
&cardnumb,&satrec.noradNum, &satrec.inclination,
&satrec.rightAscNode,&satrec.eccentricity, &satrec.argpo, &satrec.meanAnomaly, &satrec.meanMotion,
&revnum );
else
sscanf(longstr2,"%2d %5ld %9lf %9lf %8lf %9lf %9lf %11lf %6ld \n",
&cardnumb,&satrec.noradNum, &satrec.inclination,
&satrec.rightAscNode,&satrec.eccentricity, &satrec.argpo, &satrec.meanAnomaly, &satrec.meanMotion,
&revnum );
}
// ---- find no, ndot, nddot ----
satrec.meanMotionRad = satrec.meanMotion / xpdotp; //* rad/min
satrec.nddot= satrec.nddot * pow(10.0, nexp);
satrec.bstar= satrec.bstar * pow(10.0, ibexp);
// ---- convert to sgp4 units ----
satrec.a = pow( satrec.meanMotionRad*tumin , (-2.0/3.0) );
satrec.ndot = satrec.ndot / (xpdotp*1440.0); //* ? * minperday
satrec.nddot= satrec.nddot / (xpdotp*1440.0*1440);
// ---- find standard orbital elements ----
satrec.inclination = satrec.inclination * deg2rad;
satrec.rightAscNode = satrec.rightAscNode * deg2rad;
satrec.argpo = satrec.argpo * deg2rad;
satrec.meanAnomaly = satrec.meanAnomaly * deg2rad;
satrec.alta = satrec.a*(1.0 + satrec.eccentricity) - 1.0;
satrec.altp = satrec.a*(1.0 - satrec.eccentricity) - 1.0;
// ----------------------------------------------------------------
// find sgp4epoch time of element set
// remember that sgp4 uses units of days from 0 jan 1950 (sgp4epoch)
// and minutes from the epoch (time)
// ----------------------------------------------------------------
// ---------------- temp fix for years from 1957-2056 -------------------
// --------- correct fix will occur when year is 4-digit in tle ---------
if (satrec.epochyr < 57)
year= satrec.epochyr + 2000;
else
year= satrec.epochyr + 1900;
days2mdhms ( year,satrec.epochdays, mon,day,hr,minute,sec );
jday( year,mon,day,hr,minute,sec, satrec.jdsatepoch );
// ---- input start stop times manually
if ((typerun != 'v') && (typerun != 'c'))
{
// ------------- enter start/stop ymd hms values --------------------
if (typeinput == 'e')
{
printf("input start prop year mon day hr min sec \n");
// make sure there is no space at the end of the format specifiers in scanf!
scanf( "%i %i %i %i %i %lf",&startyear, &startmon, &startday, &starthr, &startmin, &startsec);
fflush(stdin);
jday( startyear,startmon,startday,starthr,startmin,startsec, jdstart );
printf("input stop prop year mon day hr min sec \n");
scanf( "%i %i %i %i %i %lf",&stopyear, &stopmon, &stopday, &stophr, &stopmin, &stopsec);
fflush(stdin);
jday( stopyear,stopmon,stopday,stophr,stopmin,stopsec, jdstop );
startmfe = (jdstart - satrec.jdsatepoch) * 1440.0;
stopmfe = (jdstop - satrec.jdsatepoch) * 1440.0;
printf("input time step in minutes \n");
scanf( "%lf",&deltamin );
}
// -------- enter start/stop year and days of year values -----------
if (typeinput == 'd')
{
printf("input start year dayofyr \n");
scanf( "%i %lf",&startyear, &startdayofyr );
printf("input stop year dayofyr \n");
scanf( "%i %lf",&stopyear, &stopdayofyr );
days2mdhms ( startyear,startdayofyr, mon,day,hr,minute,sec );
jday( startyear,mon,day,hr,minute,sec, jdstart );
days2mdhms ( stopyear,stopdayofyr, mon,day,hr,minute,sec );
jday( stopyear,mon,day,hr,minute,sec, jdstop );
startmfe = (jdstart - satrec.jdsatepoch) * 1440.0;
stopmfe = (jdstop - satrec.jdsatepoch) * 1440.0;
printf("input time step in minutes \n");
scanf( "%lf",&deltamin );
}
// ------------------ enter start/stop mfe values -------------------
if (typeinput == 'm')
{
printf("input start min from epoch \n");
scanf( "%lf",&startmfe );
printf("input stop min from epoch \n");
scanf( "%lf",&stopmfe );
printf("input time step in minutes \n");
scanf( "%lf",&deltamin );
}
}
// ------------ perform complete catalog evaluation, -+ 1 day -----------
if (typerun == 'c')
{
startmfe = -1440.0;
stopmfe = 1440.0;
deltamin = 10.0;
}
// ---------------- initialize the orbit at sgp4epoch -------------------
sgp4init( whichconst, opsmode, satrec.noradNum, satrec.jdsatepoch-2433281.5,
satrec.bstar, satrec.eccentricity, satrec.argpo, satrec.inclination,
satrec.meanAnomaly, satrec.meanMotionRad, satrec.rightAscNode, satrec);
} // end twoline2rv
/* ��GPS�������������� */
void GetTLEFromGPS(elsetrec *satrecFromGPS,double orbInfoGPS[6],double *tTime)
{
double MeanKepler[6];
double loops,RMS,PosNorm[3],endwhile;
double PosInTEME[3],VelInTEME[3];
double error[6],JToTEME[3][3],TEMEToJ[3][3];
double PosInJ[3],VelInJ[3],meanorbInfo[6];
int i,j,k,m;
cordMtxJToTEMEGet(JToTEME,tTime);
mtxInv((double *)TEMEToJ,(double *)JToTEME,3);
for (m=0;m<6;m++)
{
meanorbInfo[m] = orbInfoGPS[m];
}
Get_KplInfo(MeanKepler,meanorbInfo);
satrecFromGPS->bstar = 0.0002;
satrecFromGPS->inclo = MeanKepler[2]*PI/180;
satrecFromGPS->nodeo = MeanKepler[3]*PI/180;
satrecFromGPS->ecco = MeanKepler[1];
satrecFromGPS->argpo = MeanKepler[4]*PI/180;
satrecFromGPS->mo = MeanKepler[5]*PI/180;
satrecFromGPS->no = 60*sqrt(GM/(MeanKepler[0]*MeanKepler[0]*MeanKepler[0]));
loops = 0;
RMS = 10000;
endwhile = 1;
while (endwhile !=0)
{
loops = loops+1;
sgp4init(satrecFromGPS);
sgp4(PosInTEME,VelInTEME,satrecFromGPS,0);
mtxMtp((double *)PosInJ,(double *)TEMEToJ,3,3,(double *)PosInTEME,3,1);
mtxMtp((double *)VelInJ,(double *)TEMEToJ,3,3,(double *)VelInTEME,3,1);
for (i=0;i<3;i++)
{
error[i] = orbInfoGPS[i]-PosInJ[i];
PosNorm[i] = error[i];
}
for (j=0;j<3;j++)
{
error[j+3] = orbInfoGPS[j+3]-VelInJ[j];
}
RMS = sqrt(norm(PosNorm,3)/3);
if ((RMS >= 0.01) && (loops <= 20))
{
for (k=0;k<6;k++)
{
meanorbInfo[k] = meanorbInfo[k]+error[k];
}
Get_KplInfo(MeanKepler,meanorbInfo);
satrecFromGPS->bstar = 0.0002;
satrecFromGPS->inclo = MeanKepler[2]*PI/180;
satrecFromGPS->nodeo = MeanKepler[3]*PI/180;
satrecFromGPS->ecco = MeanKepler[1];
satrecFromGPS->argpo = MeanKepler[4]*PI/180;
satrecFromGPS->mo = MeanKepler[5]*PI/180;
satrecFromGPS->no = 60*sqrt(GM/(MeanKepler[0]*MeanKepler[0]*MeanKepler[0]));
}
else
{
endwhile = 0;
}
}
return;
}
