/* tle_pos() accuracy --------------------------------------------------------*/
static void utest3(void)
{
const char *file1="../data/tle/brdc3050.12*";
const char *file2="../data/tle/TLE_GNSS_20121101.txt";
const char *file3="../data/tle/igs17127.erp";
const double ep[6]={2012,10,31,0,0,0};
nav_t nav={0};
erp_t erp={0};
tle_t tle={0};
gtime_t time;
char sat[32];
double rs1[6],rs2[6],ds[6],dts[2],var;
int i,j,k,stat,svh;
readrnx(file1,0,"",NULL,&nav,NULL);
assert(nav.n>0);
stat=readerp(file3,&erp);
assert(stat);
stat=tle_read(file2,&tle);
assert(stat);
for (i=0;i<MAXSAT;i++) {
satno2id(i+1,sat);
fprintf(OUT,"SAT=%s\n",sat);
for (j=0;j<96;j++) {
time=timeadd(epoch2time(ep),900.0*j);
if (!satpos(time,time,i+1,EPHOPT_BRDC,&nav,rs1,dts,&var,&svh)) continue;
if (satsys(i+1,NULL)==SYS_QZS) svh&=0xFE;
if (svh) continue;
stat=tle_pos(time,sat,"","",&tle,&erp,rs2);
assert(stat);
for (k=0;k<3;k++) ds[k]=rs2[k]-rs1[k];
fprintf(OUT,"%6.0f %11.3f %11.3f %11.3f %11.3f\n",900.0*j,
ds[0]/1e3,ds[1]/1e3,ds[2]/1e3,norm(ds,3)/1e3);
assert(norm(ds,3)/1e3<300.0);
}
fprintf(OUT,"\n");
}
fprintf(OUT,"%s utest3 : OK\n",__FILE__);
}
/* satellite positions and clocks ----------------------------------------------
* compute satellite positions, velocities and clocks
* args : gtime_t teph I time to select ephemeris (gpst)
* obsd_t *obs I observation data
* int n I number of observation data
* nav_t *nav I navigation data
* int ephopt I ephemeris option (EPHOPT_???)
* double *rs O satellite positions and velocities (ecef)
* double *dts O satellite clocks
* double *var O sat position and clock error variances (m^2)
* int *svh O sat health flag (-1:correction not available)
* return : none
* notes : rs [(0:2)+i*6]= obs[i] sat position {x,y,z} (m)
* rs [(3:5)+i*6]= obs[i] sat velocity {vx,vy,vz} (m/s)
* dts[(0:1)+i*2]= obs[i] sat clock {bias,drift} (s|s/s)
* var[i] = obs[i] sat position and clock error variance (m^2)
* svh[i] = obs[i] sat health flag
* if no navigation data, set 0 to rs[], dts[], var[] and svh[]
* satellite position and clock are values at signal transmission time
* satellite position is referenced to antenna phase center
* satellite clock does not include code bias correction (tgd or bgd)
* any pseudorange and broadcast ephemeris are always needed to get
* signal transmission time
*-----------------------------------------------------------------------------*/
extern void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav,
int ephopt, double *rs, double *dts, double *var, int *svh)
{
gtime_t time[MAXOBS]={{0}};
double dt,pr;
int i,j;
trace(3,"satposs : teph=%s n=%d ephopt=%d\n",time_str(teph,3),n,ephopt);
for (i=0;i<n&&i<MAXOBS;i++) {
for (j=0;j<6;j++) rs [j+i*6]=0.0;
for (j=0;j<2;j++) dts[j+i*2]=0.0;
var[i]=0.0; svh[i]=0;
/* search any psuedorange */
for (j=0,pr=0.0;j<NFREQ;j++) if ((pr=obs[i].P[j])!=0.0) break;
if (j>=NFREQ) {
trace(2,"no pseudorange %s sat=%2d\n",time_str(obs[i].time,3),obs[i].sat);
continue;
}
/* transmission time by satellite clock */
time[i]=timeadd(obs[i].time,-pr/CLIGHT);
/* satellite clock bias by broadcast ephemeris */
if (!ephclk(time[i],teph,obs[i].sat,nav,&dt)) {
trace(2,"no broadcast clock %s sat=%2d\n",time_str(time[i],3),obs[i].sat);
continue;
}
time[i]=timeadd(time[i],-dt);
/* satellite position and clock at transmission time */
if (!satpos(time[i],teph,obs[i].sat,ephopt,nav,rs+i*6,dts+i*2,var+i,
svh+i)) {
trace(2,"no ephemeris %s sat=%2d\n",time_str(time[i],3),obs[i].sat);
continue;
}
/* if no precise clock available, use broadcast clock instead */
if (dts[i*2]==0.0) {
if (!ephclk(time[i],teph,obs[i].sat,nav,dts+i*2)) continue;
dts[1+i*2]=0.0;
*var=SQR(STD_BRDCCLK);
}
}
for (i=0;i<n&&i<MAXOBS;i++) {
trace(4,"%s sat=%2d rs=%13.3f %13.3f %13.3f dts=%12.3f var=%7.3f svh=%02X\n",
time_str(time[i],6),obs[i].sat,rs[i*6],rs[1+i*6],rs[2+i*6],
dts[i*2]*1E9,var[i],svh[i]);
}
}
/* -- void satposs(gtime_t teph,const obsd_t *obs,int n,const nav_t *nav,
double *rs, double *dts, double *var, int *svh) --------------------------------------
*
* Description :
* Parameters : *rs O satellite positions and velocities (ecef), size n
* *dts O sat clocks
* *var O sat position and clock error variances (m^2)
* *svh O sat health flag
* Return :
*/
void satposs(gtime_t teph,const obsd_t *obs,int n,const nav_t *nav,
double *rs, double *dts, double *var, int *svh,char **msg)
{
gtime_t time[MAX_OBS]={{0}};
double dt;
int i,j;
for (i=0;(i<n)&&(i<MAX_OBS);i++)
{
for (j=0;j<6;j++) rs[j+i*6]=0.0;
for (j=0;j<2;j++) dts[j+i*2]=0.0;
var[i]=0.0;svh[i]=0.0;
if (obs[i].P==0.0)
{
continue;
}
/* transmission time by satellite clock */
time[i]=timeadd(obs[i].time,-obs[i].P/CLIGHT);
/* satellite clock bias by broadcast ephemeris */
if (!ephclk(time[i],teph,obs[i].sat,nav,&dt,msg)) {
continue;
}
//*msg += sprintf(*msg,"clk");
time[i]=timeadd(time[i],-dt);
//start = TIM2->CNT;
/* satellite position and clock at transmission time */
if (!satpos(time[i],teph,obs[i].sat,nav,rs+i*6,dts+i*2,var+i,
svh+i,msg)) {
continue;
}
//if (i==0)
// SendIntStr(TIM2->CNT-start);
//*msg += sprintf(*msg,"satpos");
//if no precise clock available, use broadcast clock instead
if (dts[i*2]==0.0)
{
if (!ephclk(time[i],teph,obs[i].sat,nav,dts+i*2,msg))
continue;
dts[1+i*2]=0.0;
*var=SQR(STD_BRDCCLK);
}
}
}
// update observation data index, azimuth/elevation, satellite list ---------
void __fastcall TPlot::UpdateObs(int nobs)
{
AnsiString s;
prcopt_t opt=prcopt_default;
gtime_t time;
sol_t sol={0};
double pos[3],rr[3],e[3],azel[MAXOBS*2]={0},rs[6],dts[2],var;
int i,j,k,svh,per,per_=-1;
char msg[128];
trace(3,"UpdateObs\n");
delete [] IndexObs; IndexObs=NULL;
delete [] Az; Az=NULL;
delete [] El; El=NULL;
NObs=0;
if (nobs<=0) return;
IndexObs=new int[nobs];
Az=new double[Obs.n];
El=new double[Obs.n];
opt.err[0]=900.0;
ReadWaitStart();
ShowLegend(NULL);
for (i=0;i<Obs.n;i=j) {
time=Obs.data[i].time;
for (j=i;j<Obs.n;j++) {
if (timediff(Obs.data[j].time,time)>TTOL) break;
}
IndexObs[NObs++]=i;
if (Nav.n<=0&&Nav.ng<=0&&Nav.ns<=0) {
for (k=0;k<j-i;k++) Az[i+k]=El[i+k]=0.0;
continue;
}
if (RcvPos==0) {
pntpos(Obs.data+i,j-i,&Nav,&opt,&sol,azel,NULL,msg);
}
else {
if (RcvPos==1) { // lat/lon/height
for (k=0;k<3;k++) pos[k]=OOPos[k];
pos2ecef(pos,rr);
}
else { // rinex header position
for (k=0;k<3;k++) rr[k]=Sta.pos[k];
ecef2pos(rr,pos);
}
for (k=0;k<j-i;k++) {
azel[k*2]=azel[1+k*2]=0.0;
if (!satpos(time,time,Obs.data[i+k].sat,EPHOPT_BRDC,&Nav,rs,dts,
&var,&svh)) continue;
if (geodist(rs,rr,e)>0.0) satazel(pos,e,azel+k*2);
}
}
for (k=0;k<j-i;k++) {
Az[i+k]=azel[ k*2];
El[i+k]=azel[1+k*2];
if (Az[i+k]<0.0) Az[i+k]+=2.0*PI;
}
per=(i+1)*100/Obs.n;
if (per!=per_) {
ShowMsg(s.sprintf("updating azimuth/elevation... (%d%%)",(per_=per)));
Application->ProcessMessages();
}
}
IndexObs[NObs]=Obs.n;
UpdateSatList();
ReadWaitEnd();
}
/* generate simulated observation data ---------------------------------------*/
static int simobs(gtime_t ts, gtime_t te, double tint, const double *rr,
nav_t *nav, obs_t *obs, int opt)
{
gtime_t time;
obsd_t data[MAXSAT]={{{0}}};
double pos[3],rs[3*MAXSAT],dts[MAXSAT],r,e[3],azel[2];
double ecp[MAXSAT][NFREQ]={{0}},epr[MAXSAT][NFREQ]={{0}};
double snr[MAXSAT][NFREQ]={{0}},ers[MAXSAT][3]={{0}};
double iono,trop,fact,cp,pr,dtr=0.0,rref[3],bl;
int i,j,k,n,ns,amb[MAXSAT][NFREQ]={{0}},sys,prn;
char s[64];
double pref[]={36.106114294,140.087190410,70.3010}; /* ref station */
trace(3,"simobs:nnav=%d ngnav=%d\n",nav->n,nav->ng);
for (i=0;i<2;i++) pref[i]*=D2R;
pos2ecef(pref,rref);
for (i=0;i<3;i++) rref[i]-=rr[i];
bl=norm(rref,3)/1E4; /* baseline (10km) */
srand(0);
/* ephemeris error */
for (i=0;i<MAXSAT;i++) {
data[i].sat=i+1;
data[i].P[0]=2E7;
for (j=0;j<3;j++) ers[i][j]=randn(0.0,erreph);
}
srand(tickget());
ecef2pos(rr,pos);
n=(int)(timediff(te,ts)/tint+1.0);
for (i=0;i<n;i++) {
time=timeadd(ts,tint*i);
time2str(time,s,0);
for (j=0;j<MAXSAT;j++) data[j].time=time;
for (j=0;j<3;j++) { /* iteration for pseudorange */
satpos(time,data,MAXSAT,nav,rs,dts);
for (k=0;k<MAXSAT;k++) {
if ((r=geodist(rs+k*3,rr,e))<=0.0) continue;
data[k].P[0]=r+CLIGHT*(dtr-dts[k]);
}
}
satpos(time,data,MAXSAT,nav,rs,dts);
for (j=ns=0;j<MAXSAT;j++) {
/* add ephemeris error */
for (k=0;k<3;k++) rs[k+j*3]+=ers[j][k];
if ((r=geodist(rs+j*3,rr,e))<=0.0) continue;
satazel(pos,e,azel);
if (azel[1]<minel*D2R) continue;
iono=ionmodel(time,nav->ion,pos,azel);
trop=tropmodel(pos,azel,0.3);
/* add ionospheric error */
iono+=errion*bl*ionmapf(pos,azel);
snrmodel(azel,snr[j]);
errmodel(azel,snr[j],ecp[j],epr[j]);
sys=satsys(data[j].sat,&prn);
for (k=0;k<NFREQ;k++) {
data[j].L[k]=data[j].P[k]=0.0;
data[j].SNR[k]=0;
data[j].LLI[k]=0;
if (sys==SYS_GPS) {
if (k>=3) continue; /* no L5a/L5b in gps */
if (k>=2&&!gpsblock[prn-1]) continue; /* no L5 in block II */
}
else if (sys==SYS_GLO) {
if (k>=3) continue;
}
else if (sys==SYS_GAL) {
if (k==1) continue; /* no L2 in galileo */
}
else continue;
/* generate observation data */
fact=lam[k]*lam[k]/lam[0]/lam[0];
cp=r+CLIGHT*(dtr-dts[j])-fact*iono+trop+ecp[j][k];
pr=r+CLIGHT*(dtr-dts[j])+fact*iono+trop+epr[j][k];
if (amb[j][k]==0) amb[j][k]=(int)(-cp/lam[k]);
data[j].L[k]=cp/lam[k]+amb[j][k];
data[j].P[k]=pr;
data[j].SNR[k]=(unsigned char)snr[j][k];
data[j].LLI[k]=data[j].SNR[k]<slipthres?1:0;
}
if (obs->nmax<=obs->n) {
if (obs->nmax==0) obs->nmax=65532; else obs->nmax+=65532;
if (!(obs->data=(obsd_t *)realloc(obs->data,sizeof(obsd_t)*obs->nmax))) {
fprintf(stderr,"malloc error\n");
return 0;
}
}
obs->data[obs->n++]=data[j];
ns++;
}
fprintf(stderr,"time=%s nsat=%2d\r",s,ns);
}
fprintf(stderr,"\n");
return 1;
}
