/* single-point positioning ----------------------------------------------------
* compute receiver position, velocity, clock bias by single-point positioning
* with pseudorange and doppler observables
* args : obsd_t *obs I observation data
* int n I number of observation data
* nav_t *nav I navigation data
* prcopt_t *opt I processing options
* sol_t *sol IO solution
* double *azel IO azimuth/elevation angle (rad) (NULL: no output)
* ssat_t *ssat IO satellite status (NULL: no output)
* char *msg O error message for error exit
* return : status(1:ok,0:error)
* notes : assuming sbas-gps, galileo-gps, qzss-gps, compass-gps time offset and
* receiver bias are negligible (only involving glonass-gps time offset
* and receiver bias)
*-----------------------------------------------------------------------------*/
extern int pntpos(const obsd_t *obs, int n, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
char *msg)
{
prcopt_t opt_=*opt;
double *rs,*dts,*var,*azel_,*resp;
int i,stat,vsat[MAXOBS]={0},svh[MAXOBS];
sol->stat=SOLQ_NONE;
if (n<=0) {strcpy(msg,"no observation data"); return 0;}
trace(3,"pntpos : tobs=%s n=%d\n",time_str(obs[0].time,3),n);
sol->time=obs[0].time; msg[0]='\0';
rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel_=zeros(2,n); resp=mat(1,n);
if (opt_.mode!=PMODE_SINGLE) { /* for precise positioning */
#if 0
opt_.sateph =EPHOPT_BRDC;
#endif
opt_.ionoopt=IONOOPT_BRDC;
opt_.tropopt=TROPOPT_SAAS;
}
/* satellite positons, velocities and clocks */
satposs(sol->time,obs,n,nav,opt_.sateph,rs,dts,var,svh);
/* estimate receiver position with pseudorange */
stat=estpos(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
/* raim fde */
if (!stat&&n>=6&&opt->posopt[4]) {
stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
}
/* estimate receiver velocity with doppler */
if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat);
if (azel) {
for (i=0;i<n*2;i++) azel[i]=azel_[i];
}
if (ssat) {
for (i=0;i<MAXSAT;i++) {
ssat[i].vs=0;
ssat[i].azel[0]=ssat[i].azel[1]=0.0;
ssat[i].resp[0]=ssat[i].resc[0]=0.0;
ssat[i].snr[0]=0;
}
for (i=0;i<n;i++) {
ssat[obs[i].sat-1].azel[0]=azel_[ i*2];
ssat[obs[i].sat-1].azel[1]=azel_[1+i*2];
ssat[obs[i].sat-1].snr[0]=obs[i].SNR[0];
if (!vsat[i]) continue;
ssat[obs[i].sat-1].vs=1;
ssat[obs[i].sat-1].resp[0]=resp[i];
}
}
free(rs); free(dts); free(var); free(azel_); free(resp);
return stat;
}
/* estimate ionosphere -------------------------------------------------------*/
static int est_iono(obs_t *obs, nav_t *nav, const pcv_t *pcv, double *rr,
double tint, FILE *fp)
{
sstat_t sstat[MAXSAT]={{{0}}};
ekf_t *ekf;
gtime_t time;
double pos[3],rs[MAXOBS*6],dts[MAXOBS*2],var[MAXOBS],e[3],azel[MAXOBS*2];
double *v,*H,*R,phw[MAXSAT]={0};
int i,j,n=0,info,nx=NX,nv=MAXSAT*2,svh[MAXOBS];
ekf=ekf_new(NX); v=mat(nv,1); H=mat(nx,nv); R=mat(nv,nv);
/* receiver position */
ecef2pos(rr,pos);
out_head(obs->data[0].time,pos,fp);
for (i=0;i<obs->n;i+=n) {
for (n=1;i+n<obs->n;n++) {
if (timediff(obs->data[i+n].time,obs->data[i].time)>1E-3) break;
}
time=obs->data[i].time;
/* satellite positions and clocks */
satposs(time,obs->data+i,n,nav,EPHOPT_BRDC,rs,dts,var,svh);
/* satellite azimuth/elevation angle */
for (j=0;j<n;j++) {
if (geodist(rs+j*6,rr,e)>0.0) satazel(pos,e,azel+j*2);
else azel[j*2]=azel[1+j*2]=0.0;
}
/* time update of parameters */
ud_state(obs->data+i,n,nav,pos,azel,ekf,sstat);
/* ionosphere residuals */
if ((nv=res_iono(obs->data+i,n,nav,rs,rr,pos,azel,pcv,ekf,phw,v,H,R))<=0) {
continue;
}
/* filter */
if ((info=filter(ekf->x,ekf->P,H,v,R,ekf->nx,nv))) {
fprintf(stderr,"filter error: info=%d\n",info);
break;
}
/* output ionopshere parameters */
if (tint<=0.0||fmod(time2gpst(time,NULL)+0.005,tint)<0.01) {
out_iono(obs->data[i].time,ekf,sstat,fp);
}
}
ekf_free(ekf); free(v); free(H); free(R);
return 1;
}
/* satellite azimuth/elevation angle -----------------------------------------*/
static void sat_azel(const obsd_t *obs, int n, const nav_t *nav,
const double *pos, double *azel)
{
double rs[MAXOBS*6],dts[MAXOBS*2],var[MAXOBS],r,e[3];
int svh[MAXOBS];
/* satellite positions and clocks */
satposs(obs[0].time,obs,n,nav,EPHOPT_BRDC,rs,dts,var,svh);
for (i=0;i<n;i++) {
if (geodist(rs+i*6,rr,e))>0.0) satazel(pos,e,azel+i*2);
}
}
//SPP, added by yuan;
extern int pntpos(rtk_t *rtk,obsd_t *obs, int n, nav_t *nav,double *azel,char *msg)
{
prcopt_t opt_=rtk->opt;
int i,stat,vsat[MAXOBS]={0},svh[MAXOBS];
double rs[6 * MAXOBS_]; double dts[2 * MAXOBS_]; double var[1 * MAXOBS_];
double azel_[2 * MAXOBS_]; double resp[1 * MAXOBS_];
rtk->sol.stat=SOLQ_NONE;
if (n<=0) { return 0;}
rtk->sol.time = obs[0].time; msg[0] = '\0';
opt_.sateph =EPHOPT_BRDC; //���ù㲥������modified by yuan
opt_.ionoopt=IONOOPT_BRDC;
opt_.tropopt=TROPOPT_SAAS;
/* satellite positons, velocities and clocks */
satposs(rtk->sol.time, obs, n, nav, opt_.sateph, rs, dts, var, svh);
/* estimate receiver position with pseudorange */
stat = estpos(obs, n, rs, dts, var, svh, nav, &opt_, &rtk->sol, azel_, vsat, resp, msg, rtk);
if (azel) {
for (i=0;i<n*2;i++) azel[i]=azel_[i];
}
if (rtk->ssat) {
for (i=0;i<MAXSAT;i++) {
rtk->ssat[i].vs = 0;
rtk->ssat[i].azel[0] = rtk->ssat[i].azel[1] = 0.0;
rtk->ssat[i].resp[0] = rtk->ssat[i].resc[0] = 0.0;
rtk->ssat[i].snr[0] = 0;
}
for (i=0;i<n;i++) {
rtk->ssat[obs[i].sat - 1].azel[0] = azel_[i * 2];
rtk->ssat[obs[i].sat - 1].azel[1] = azel_[1 + i * 2];
rtk->ssat[obs[i].sat - 1].snr[0] = obs[i].SNR[0];
if (!vsat[i]) continue;
rtk->ssat[obs[i].sat - 1].vs = 1;
rtk->ssat[obs[i].sat - 1].resp[0] = resp[i];
}
}
return stat;
}
//0:ok, !0: not ok
static int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,
const nav_t *nav,char **msg)
{
prcopt_t *opt=&rtk->opt;
gtime_t time=obs[0].time;
double *rs,*dts,*var,*y,*e,*azel,*v,*H,*R,*bias,dt;
double *xp,*Pp,*xa;
int i,n=nu+nr,ns,ny,nv,sat[MAX_SAT],iu[MAX_SAT],ir[MAX_SAT];
int info,vflg[MAX_OBS*2+1],svh[MAX_OBS*2];
int stat=SOLQ_FLOAT;
dt=timediff(time,obs[nu].time);
rs=mat(6,n);
dts=mat(2,n);
var=mat(1,n);
y=mat(2,n);
e=mat(3,n);
azel=zeros(2,n);
if ((!rs)||(!dts)||(!var)||(!y)||(!e)||(!azel))
{
#ifdef _DEBUG_MSG
(*msg)+=sprintf(*msg,"relpos mat error 1\n");
#endif
free(rs);
free(dts);
free(var);
free(y);
free(e);
free(azel);
return -1;
}
for (i=0; i<MAX_SAT; i++)
{
rtk->ssat[i].vsat = rtk->ssat[i].snr=0;
}
/* satellite positions/clocks*/
satposs(time,obs,n,nav,rs,dts,var,svh,msg); //da tinh trong pntpos rui**************************
/* undifferenced residuals for base station */
if (zdres(1,obs+nu,nr,rs+nu*6,dts+nu*2,svh+nu,nav,rtk->rb,opt,1,
y+nu*2,e+nu*3,azel+nu*2)) //*****************************************
{
#ifdef _DEBUG_MSG
(*msg)+=sprintf(*msg,"initial base station position error\n");
#endif
free(rs);
free(dts);
free(var);
free(y);
free(e);
free(azel);
return -1;
}//*********************88
/* select common satellites between rover and base-station */
if ((ns=selsat(obs,azel,nu,nr,opt,sat,iu,ir,msg))<=0) {//check!!!!!!!!!!!!!!!*************************************
#ifdef _DEBUG_MSG
(*msg)+=sprintf(*msg,"no common satellite\n");
#endif
free(rs);
free(dts);
free(var);
free(y);
free(e);
free(azel);
return -1;
}
// if (ns>7)
// ns=7;
// (*msg)+=sprintf(*msg,"common satellite:%d\n",ns);
/* temporal update of states *///***************************************************************************
udstate(rtk,obs,sat,iu,ir,ns,nav,msg);//*********************************************
xp=mat(rtk->nx,1);
Pp=zeros(rtk->nx,rtk->nx);
xa=mat(rtk->nx,1);
matcpy(xp,rtk->x,rtk->nx,1);
ny=ns*2+2;
v=mat(ny,1);
H=zeros(rtk->nx,ny);
R=mat(ny,ny);
bias=mat(rtk->nx,1);
if ((!xp)||(!Pp)||(!xa)||(!v)||(!H)||(!R)||(!bias))
{
#ifdef _DEBUG_MSG
(*msg)+=sprintf(*msg,"relpos mat error 2\n");
#endif
free(rs);
free(dts);
free(var);
free(y);
free(e);
free(azel);
free(xp);
free(Pp);
free(xa);
free(v);
free(H);
free(R);
free(bias);
return -1;
}
//*************************************************8
for (i=0; i<NITER; i++) {
/* undifferenced residuals for rover */
if (zdres(0,obs,nu,rs,dts,svh,nav,xp,opt,0,y,e,azel)) {
#ifdef _DEBUG_MSG
(*msg)+=sprintf(*msg,"rover initial position error\n");
#endif
stat=SOLQ_NONE;
break;
}
//*************************
/* double-differenced residuals and partial derivatives */
if ((nv=ddres(rtk,nav,dt,xp,Pp,sat,y,e,azel,iu,ir,ns,v,H,R,vflg,msg))<1) {
#ifdef _DEBUG_MSG
(*msg)+=sprintf(*msg,"no double-differenced residual\n");
#endif
stat=SOLQ_NONE;
break;
}
/* kalman filter measurement update */
matcpy(Pp,rtk->P,rtk->nx,rtk->nx);
if ((info=filter(xp,Pp,H,v,R,rtk->nx,nv))) {
#ifdef _DEBUG_MSG
(*msg)+=sprintf(*msg,"filter error (info=%d)\n",info);
#endif
stat=SOLQ_NONE;
break;
}
}
//***************************
if (stat!=SOLQ_NONE&&(!zdres(0,obs,nu,rs,dts,svh,nav,xp,opt,0,y,e,azel))) {
/* post-fit residuals for float solution */
nv=ddres(rtk,nav,dt,xp,Pp,sat,y,e,azel,iu,ir,ns,v,NULL,R,vflg,msg);
/* validation of float solution */
if (!valpos(rtk,v,R,vflg,nv,4.0,msg)) {//0:ok,!0:not ok
/* update state and covariance matrix */
matcpy(rtk->x,xp,rtk->nx,1);
matcpy(rtk->P,Pp,rtk->nx,rtk->nx);
/* update ambiguity control struct */
rtk->sol.ns=0;
for (i=0; i<ns; i++)
{
if (!rtk->ssat[sat[i]-1].vsat) continue;
rtk->ssat[sat[i]-1].lock++;
rtk->ssat[sat[i]-1].outc=0;
rtk->sol.ns++; /* valid satellite count by L1 */
}
/* lack of valid satellites */
if (rtk->sol.ns<4) stat=SOLQ_NONE;
}
else stat=SOLQ_NONE;
}
/* resolve integer ambiguity by LAMBDA */
if (stat!=SOLQ_NONE&&(resamb_LAMBDA(rtk,bias,xa,msg)>1)) {
if (!zdres(0,obs,nu,rs,dts,svh,nav,xa,opt,0,y,e,azel)) {
/* post-fit reisiduals for fixed solution */
nv=ddres(rtk,nav,dt,xa,NULL,sat,y,e,azel,iu,ir,ns,v,NULL,R,vflg,msg);
/* validation of fixed solution */
if (!valpos(rtk,v,R,vflg,nv,4.0,msg)) {
/* hold integer ambiguity */
if (++rtk->nfix>=rtk->opt.minfix&&
rtk->opt.modear==ARMODE_FIXHOLD) {
holdamb(rtk,xa,msg);
}
stat=SOLQ_FIX;
}
}
}
/* save solution status */
if (stat==SOLQ_FIX) {
for (i=0; i<3; i++) {
rtk->sol.rr[i]=rtk->xa[i];
rtk->sol.qr[i]=(float)rtk->Pa[i+i*rtk->na];
}
rtk->sol.qr[3]=(float)rtk->Pa[1];
rtk->sol.qr[4]=(float)rtk->Pa[1+2*rtk->na];
rtk->sol.qr[5]=(float)rtk->Pa[2];
}
else {
for (i=0; i<3; i++) {
rtk->sol.rr[i]=rtk->x[i];
rtk->sol.qr[i]=(float)rtk->P[i+i*rtk->nx];
}
rtk->sol.qr[3]=(float)rtk->P[1];
rtk->sol.qr[4]=(float)rtk->P[1+2*rtk->nx];
rtk->sol.qr[5]=(float)rtk->P[2];
rtk->nfix=0;
}
for (i=0; i<n; i++) {
if (obs[i].L==0.0) continue;
rtk->ssat[obs[i].sat-1].pt[obs[i].rcv-1]=obs[i].time;
rtk->ssat[obs[i].sat-1].ph[obs[i].rcv-1]=obs[i].L;
}
for (i=0; i<ns; i++) {
/* output snr of rover receiver */
rtk->ssat[sat[i]-1].snr=obs[iu[i]].SNR;
}
for (i=0; i<MAX_SAT; i++) {
if (rtk->ssat[i].fix==2&&stat!=SOLQ_FIX) rtk->ssat[i].fix=1;
if (rtk->ssat[i].slip&1) rtk->ssat[i].slipc++;
}
free(rs);
free(dts);
free(var);
free(y);
free(e);
free(azel);
free(xp);
free(Pp);
free(xa);
free(v);
free(H);
free(R);
free(bias);
if (stat!=SOLQ_NONE) rtk->sol.stat=stat;
return stat==SOLQ_NONE;
}
/* phase and code residuals --------------------------------------------------*/
static int res_ppp(int iter, const obsd_t *obs, int n, const double *rs,
const double *dts, const double *vare, const int *svh,
const nav_t *nav, const double *x, rtk_t *rtk, double *v,
double *H, double *R, double *azel)
{
prcopt_t *opt=&rtk->opt;
double r,rr[3],disp[3],pos[3],e[3],meas[2],dtdx[3],dantr[NFREQ]={0};
double dants[NFREQ]={0},var[MAXOBS*2],dtrp=0.0,vart=0.0,varm[2]={0};
int i,j,k,sat,sys,nv=0,nx=rtk->nx,brk,tideopt;
trace(3,"res_ppp : n=%d nx=%d\n",n,nx);
for (i=0;i<MAXSAT;i++) rtk->ssat[i].vsat[0]=0;
for (i=0;i<3;i++) rr[i]=x[i];
/* earth tides correction */
if (opt->tidecorr) {
tideopt=opt->tidecorr==1?1:7; /* 1:solid, 2:solid+otl+pole */
tidedisp(gpst2utc(obs[0].time),rr,tideopt,&nav->erp,opt->odisp[0],
disp);
for (i=0;i<3;i++) rr[i]+=disp[i];
}
ecef2pos(rr,pos);
for (i=0;i<n&&i<MAXOBS;i++) {
sat=obs[i].sat;
if (!(sys=satsys(sat,NULL))||!rtk->ssat[sat-1].vs) continue;
/* geometric distance/azimuth/elevation angle */
if ((r=geodist(rs+i*6,rr,e))<=0.0||
satazel(pos,e,azel+i*2)<opt->elmin) continue;
/* excluded satellite? */
if (satexclude(obs[i].sat,svh[i],opt)) continue;
/* tropospheric delay correction */
if (opt->tropopt==TROPOPT_SAAS) {
dtrp=tropmodel(obs[i].time,pos,azel+i*2,REL_HUMI);
vart=SQR(ERR_SAAS);
}
else if (opt->tropopt==TROPOPT_SBAS) {
dtrp=sbstropcorr(obs[i].time,pos,azel+i*2,&vart);
}
else if (opt->tropopt==TROPOPT_EST||opt->tropopt==TROPOPT_ESTG) {
dtrp=prectrop(obs[i].time,pos,azel+i*2,opt,x+IT(opt),dtdx,&vart);
}
else if (opt->tropopt==TROPOPT_COR||opt->tropopt==TROPOPT_CORG) {
dtrp=prectrop(obs[i].time,pos,azel+i*2,opt,x,dtdx,&vart);
}
/* satellite antenna model */
if (opt->posopt[0]) {
satantpcv(rs+i*6,rr,nav->pcvs+sat-1,dants);
}
/* receiver antenna model */
antmodel(opt->pcvr,opt->antdel[0],azel+i*2,opt->posopt[1],dantr);
/* phase windup correction */
if (opt->posopt[2]) {
windupcorr(rtk->sol.time,rs+i*6,rr,&rtk->ssat[sat-1].phw);
}
/* ionosphere and antenna phase corrected measurements */
if (!corrmeas(obs+i,nav,pos,azel+i*2,&rtk->opt,dantr,dants,
rtk->ssat[sat-1].phw,meas,varm,&brk)) {
continue;
}
/* satellite clock and tropospheric delay */
r+=-CLIGHT*dts[i*2]+dtrp;
trace(5,"sat=%2d azel=%6.1f %5.1f dtrp=%.3f dantr=%6.3f %6.3f dants=%6.3f %6.3f phw=%6.3f\n",
sat,azel[i*2]*R2D,azel[1+i*2]*R2D,dtrp,dantr[0],dantr[1],dants[0],
dants[1],rtk->ssat[sat-1].phw);
for (j=0;j<2;j++) { /* for phase and code */
if (meas[j]==0.0) continue;
for (k=0;k<nx;k++) H[k+nx*nv]=0.0;
v[nv]=meas[j]-r;
for (k=0;k<3;k++) H[k+nx*nv]=-e[k];
if (sys!=SYS_GLO) {
v[nv]-=x[IC(0,opt)];
H[IC(0,opt)+nx*nv]=1.0;
}
else {
v[nv]-=x[IC(1,opt)];
H[IC(1,opt)+nx*nv]=1.0;
}
if (opt->tropopt>=TROPOPT_EST) {
for (k=0;k<(opt->tropopt>=TROPOPT_ESTG?3:1);k++) {
H[IT(opt)+k+nx*nv]=dtdx[k];
}
}
if (j==0) {
v[nv]-=x[IB(obs[i].sat,opt)];
H[IB(obs[i].sat,opt)+nx*nv]=1.0;
}
var[nv]=varerr(obs[i].sat,sys,azel[1+i*2],j,opt)+varm[j]+vare[i]+vart;
if (j==0) rtk->ssat[sat-1].resc[0]=v[nv];
else rtk->ssat[sat-1].resp[0]=v[nv];
/* test innovation */
#if 0
if (opt->maxinno>0.0&&fabs(v[nv])>opt->maxinno) {
#else
if (opt->maxinno>0.0&&fabs(v[nv])>opt->maxinno&&sys!=SYS_GLO) {
#endif
trace(2,"ppp outlier rejected %s sat=%2d type=%d v=%.3f\n",
time_str(obs[i].time,0),sat,j,v[nv]);
rtk->ssat[sat-1].rejc[0]++;
continue;
}
if (j==0) rtk->ssat[sat-1].vsat[0]=1;
nv++;
}
}
for (i=0;i<nv;i++) for (j=0;j<nv;j++) {
R[i+j*nv]=i==j?var[i]:0.0;
}
trace(5,"x=\n"); tracemat(5,x, 1,nx,8,3);
trace(5,"v=\n"); tracemat(5,v, 1,nv,8,3);
trace(5,"H=\n"); tracemat(5,H,nx,nv,8,3);
trace(5,"R=\n"); tracemat(5,R,nv,nv,8,5);
return nv;
}
/* number of estimated states ------------------------------------------------*/
extern int pppnx(const prcopt_t *opt)
{
return NX(opt);
}
/* precise point positioning -------------------------------------------------*/
extern void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
{
const prcopt_t *opt=&rtk->opt;
double *rs,*dts,*var,*v,*H,*R,*azel,*xp,*Pp;
int i,nv,info,svh[MAXOBS],stat=SOLQ_SINGLE;
trace(3,"pppos : nx=%d n=%d\n",rtk->nx,n);
rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel=zeros(2,n);
for (i=0;i<MAXSAT;i++) rtk->ssat[i].fix[0]=0;
/* temporal update of states */
udstate_ppp(rtk,obs,n,nav);
trace(4,"x(0)="); tracemat(4,rtk->x,1,NR(opt),13,4);
/* satellite positions and clocks */
satposs(obs[0].time,obs,n,nav,rtk->opt.sateph,rs,dts,var,svh);
/* exclude measurements of eclipsing satellite */
if (rtk->opt.posopt[3]) {
testeclipse(obs,n,nav,rs);
}
xp=mat(rtk->nx,1); Pp=zeros(rtk->nx,rtk->nx);
matcpy(xp,rtk->x,rtk->nx,1);
nv=n*rtk->opt.nf*2; v=mat(nv,1); H=mat(rtk->nx,nv); R=mat(nv,nv);
for (i=0;i<rtk->opt.niter;i++) {
/* phase and code residuals */
if ((nv=res_ppp(i,obs,n,rs,dts,var,svh,nav,xp,rtk,v,H,R,azel))<=0) break;
/* measurement update */
matcpy(Pp,rtk->P,rtk->nx,rtk->nx);
if ((info=filter(xp,Pp,H,v,R,rtk->nx,nv))) {
trace(2,"ppp filter error %s info=%d\n",time_str(rtk->sol.time,0),
info);
break;
}
trace(4,"x(%d)=",i+1); tracemat(4,xp,1,NR(opt),13,4);
stat=SOLQ_PPP;
}
if (stat==SOLQ_PPP) {
/* postfit residuals */
res_ppp(1,obs,n,rs,dts,var,svh,nav,xp,rtk,v,H,R,azel);
/* update state and covariance matrix */
matcpy(rtk->x,xp,rtk->nx,1);
matcpy(rtk->P,Pp,rtk->nx,rtk->nx);
/* ambiguity resolution in ppp */
if (opt->modear==ARMODE_PPPAR||opt->modear==ARMODE_PPPAR_ILS) {
if (pppamb(rtk,obs,n,nav,azel)) stat=SOLQ_FIX;
}
/* update solution status */
rtk->sol.ns=0;
for (i=0;i<n&&i<MAXOBS;i++) {
if (!rtk->ssat[obs[i].sat-1].vsat[0]) continue;
rtk->ssat[obs[i].sat-1].lock[0]++;
rtk->ssat[obs[i].sat-1].outc[0]=0;
rtk->ssat[obs[i].sat-1].fix [0]=4;
rtk->sol.ns++;
}
rtk->sol.stat=stat;
for (i=0;i<3;i++) {
rtk->sol.rr[i]=rtk->x[i];
rtk->sol.qr[i]=(float)rtk->P[i+i*rtk->nx];
}
rtk->sol.qr[3]=(float)rtk->P[1];
rtk->sol.qr[4]=(float)rtk->P[2+rtk->nx];
rtk->sol.qr[5]=(float)rtk->P[2];
rtk->sol.dtr[0]=rtk->x[IC(0,opt)];
rtk->sol.dtr[1]=rtk->x[IC(1,opt)]-rtk->x[IC(0,opt)];
for (i=0;i<n&&i<MAXOBS;i++) {
rtk->ssat[obs[i].sat-1].snr[0]=MIN(obs[i].SNR[0],obs[i].SNR[1]);
}
for (i=0;i<MAXSAT;i++) {
if (rtk->ssat[i].slip[0]&3) rtk->ssat[i].slipc[0]++;
}
}
free(rs); free(dts); free(var); free(azel);
free(xp); free(Pp); free(v); free(H); free(R);
}
/* single-point positioning ----------------------------------------------------
* compute receiver position, velocity, clock bias by single-point positioning
* with pseudorange and doppler observables
* args : obsd_t *obs I observation data
* int n I number of observation data
* nav_t *nav I navigation data
* prcopt_t *opt I processing options
* sol_t *sol IO solution
* double *azel IO azimuth/elevation angle (rad) (NULL: no output)
* ssat_t *ssat IO satellite status (NULL: no output)
* char *msg O error message for error exit
* return : status(1:ok,0:error)
* notes : assuming sbas-gps, galileo-gps, qzss-gps, compass-gps time offset and
* receiver bias are negligible (only involving glonass-gps time offset
* and receiver bias)
*-----------------------------------------------------------------------------*/
extern int pntpos(const obsd_t *obs, int n, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
char *msg)
{
prcopt_t opt_=*opt;
double *rs,*dts,*var,*azel_,*resp;
int i,stat,vsat[MAXOBS]={0},svh[MAXOBS];
double ep[6], ep2[6];
output = fopen("solution_log.txt", "a");
fprintf(output, "\nBegin solution in single point positioning mode\n");
fprintf(output, " Input observaton data:\n");
for(i = 0; i < n; i++)
{
time2epoch(obs[i].time, ep);
fprintf(output, " %i) time: %4.0f/%2.0f/%2.0f %2.0f:%2.0f:%6.4f, sat: %i, code: %i, P1: %f, L1: %f, D1: %f, S1: %f, P2: %f, L2: %f, D2: %f, S2: %f\n", i+1, ep[0], ep[1], ep[2], ep[3], ep[4], ep[5], obs[i].sat, obs[i].code, obs[i].P[0], obs[i].L[0], obs[i].D[0], obs[i].SNR[0], obs[i].P[1], obs[i].L[1], obs[i].D[1], obs[i].SNR[1]);
}
fprintf(output, " Input GPS ephemeris:\n");
for(i = 0; i < nav->n; i++)
{
time2epoch(nav->eph[i].toe, ep);
time2epoch(nav->eph[i].toc, ep2);
fprintf(output, " %i) sat: %i, IODE: %i, sva: %i, svh: %i, week: %i, toe: %4.0f/%2.0f/%2.0f %2.0f:%2.0f:%6.4f, toc: %4.0f/%2.0f/%2.0f %2.0f:%2.0f:%6.4f, sqrt(A): %f, E: %f, i0: %f, OMG0: %f, omega: %f, M0: %f, delta_n: %f, OMGdot: %f, Idot: %f, Crc: %f, Crs: %f, Cuc: %f, Cus: %f, Cic: %f, Cis: %f, af0: %f, af1: %f, af2: %f, Tgd: %f\n",
i + 1, nav->eph[i].sat, nav->eph[i].iode, nav->eph[i].sva, nav->eph[i].svh, nav->eph[i].week,
ep[0], ep[1], ep[2], ep[3], ep[4], ep[5], ep2[0], ep2[1], ep2[2], ep2[3], ep2[4], ep2[5],
sqrt(nav->eph[i].A), nav->eph[i].e, nav->eph[i].i0, nav->eph[i].OMG0, nav->eph[i].omg, nav->eph[i].M0,
nav->eph[i].deln, nav->eph[i].OMGd, nav->eph[i].idot, nav->eph[i].crc, nav->eph[i].crs,
nav->eph[i].cuc, nav->eph[i].cus, nav->eph[i].cic, nav->eph[i].cis, nav->eph[i].f0,
nav->eph[i].f0, nav->eph[i].f1, nav->eph[i].f2, nav->eph[i].tgd[0]);
}
fprintf(output, " Input GLONASS ephemeris:\n");
for(i = 0; i < nav->ng; i++)
{
time2epoch(nav->geph[i].toe, ep);
fprintf(output, " %i) sat: %i, sva: %i, svh: %i, toe: %4.0f/%2.0f/%2.0f %2.0f:%2.0f:%6.4f, X: %f, Y: %f, Z: %f, VX: %f, VY: %f, VZ: %f, tau_n: %f, gamma_n: %f\n",
i + 1, nav->geph[i].sat, nav->geph[i].sva, nav->geph[i].svh, ep[0], ep[1], ep[2], ep[3], ep[4], ep[5],
nav->geph[i].pos[0], nav->geph[i].pos[1], nav->geph[i].pos[2],
nav->geph[i].vel[0], nav->geph[i].vel[1], nav->geph[i].vel[2], nav->geph[i].taun, nav->geph[i].gamn);
}
sol->stat=SOLQ_NONE;
if (n<=0) {strcpy(msg,"no observation data"); return 0;}
trace(3,"pntpos : tobs=%s n=%d\n",time_str(obs[0].time,3),n);
sol->time=obs[0].time; msg[0]='\0';
rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel_=zeros(2,n); resp=mat(1,n);
if (opt_.mode!=PMODE_SINGLE) { /* for precise positioning */
#if 0
opt_.sateph =EPHOPT_BRDC;
#endif
opt_.ionoopt=IONOOPT_BRDC;
opt_.tropopt=TROPOPT_SAAS;
}
/* satellite positons, velocities and clocks */
satposs(sol->time,obs,n,nav,opt_.sateph,rs,dts,var,svh);
fprintf(output, " Satellite positions, velocities and clocks:\n");
for(i = 0; i < n; i++)
{
fprintf(output, " sat %i: X = %f, Y = %f, Z = %f, VX = %f, VY = %f, VZ = %f, dt = %f, dt_dot = %f\n", obs[i].sat, rs[i*6], rs[i*6 + 1], rs[i*6 + 2], rs[i*6 + 3], rs[i*6 + 4], rs[i*6 + 5], dts[i*2], dts[i*2 + 1]);
}
/* estimate receiver position with pseudorange */
stat=estpos(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
/* raim fde */
if (!stat&&n>=6&&opt->posopt[4]) {
stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
}
/* estimate receiver velocity with doppler */
if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat);
if (azel) {
for (i=0;i<n*2;i++) azel[i]=azel_[i];
}
if (ssat) {
for (i=0;i<MAXSAT;i++) {
ssat[i].vs=0;
ssat[i].azel[0]=ssat[i].azel[1]=0.0;
ssat[i].resp[0]=ssat[i].resc[0]=0.0;
ssat[i].snr[0]=0;
}
for (i=0;i<n;i++) {
ssat[obs[i].sat-1].azel[0]=azel_[ i*2];
ssat[obs[i].sat-1].azel[1]=azel_[1+i*2];
ssat[obs[i].sat-1].snr[0]=obs[i].SNR[0];
if (!vsat[i]) continue;
ssat[obs[i].sat-1].vs=1;
ssat[obs[i].sat-1].resp[0]=resp[i];
}
}
free(rs); free(dts); free(var); free(azel_); free(resp);
fprintf(output, "End calculations in single point positioning mode\n");
fclose(output);
output = fopen("cov_matr_single.txt", "a");
time2epoch(sol->time, ep);
fprintf(output, "%4.0f/%2.0f/%2.0f %2.0f:%2.0f:%2.4f coords: %f %f %f %f %f %f velocities: %f %f %f %f %f %f\n", ep[0], ep[1], ep[2], ep[3], ep[4], ep[5], sol->qr[0], sol->qr[3], sol->qr[5], sol->qr[1], sol->qr[4], sol->qr[2], sol->qvr[0], sol->qvr[3], sol->qvr[5], sol->qvr[1], sol->qvr[4], sol->qvr[2]);
fclose(output);
return stat;
}
