/* estimate receiver position ------------------------------------------------*/
static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
const double *vare, const int *svh, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
double *resp, char *msg)
{
double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig;
int i,j,k,info,stat,nv,ns;
trace(3,"estpos : n=%d\n",n);
v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1);
for (i=0;i<3;i++) x[i]=sol->rr[i];
for (i=0;i<MAXITR;i++) {
/* pseudorange residuals */
nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,
&ns);
if (nv<NX) {
sprintf(msg,"lack of valid sats ns=%d",nv);
break;
}
/* weight by variance */
for (j=0;j<nv;j++) {
sig=sqrt(var[j]);
v[j]/=sig;
for (k=0;k<NX;k++) H[k+j*NX]/=sig;
}
/* least square estimation */
if ((info=lsq(H,v,NX,nv,dx,Q))) {
sprintf(msg,"lsq error info=%d",info);
break;
}
for (j=0;j<NX;j++) x[j]+=dx[j];
if (norm(dx,NX)<1E-4) {
sol->type=0;
sol->time=timeadd(obs[0].time,-x[3]/CLIGHT);
sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */
sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */
sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */
sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */
for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0;
for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX];
sol->qr[3]=(float)Q[1]; /* cov xy */
sol->qr[4]=(float)Q[2+NX]; /* cov yz */
sol->qr[5]=(float)Q[2]; /* cov zx */
sol->ns=(unsigned char)ns;
sol->age=sol->ratio=0.0;
/* validate solution */
if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) {
sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE;
}
free(v); free(H); free(var);
return stat;
}
}
if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i);
free(v); free(H); free(var);
return 0;
}
/* estimate receiver position ------------------------------------------------*/
static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
const double *vare, const int *svh, const nav_t *nav,
prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
double *resp, char *msg, rtk_t *rtk)
{
double x[NX]={0},dx[NX],Q[NX*NX],sig;
int i,j,k,info,stat,nv,ns;
double rms = 0; //added by yuan;
double RMS = 0;
int nv_ = 0; double temp = 0; int kk = 0;
double v[(MAXOBS_+4)*1];double H[(MAXOBS_+4)*135];double var[MAXOBS_+4*1];
for (i=0;i<3;i++) x[i]=sol->rr[i];
for (i = 0; i < MAXITR; i++) {
/* pseudorange residuals */
nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,
&ns,rtk);
if (nv<4) {
//sprintf(msg,"lack of valid sats ns=%d",nv);
break;
}
//added by yuan, ̽��ϴ�ֲ���н�ȷ�������������;
if ( (rtk->counter>=1) && (i>=1) ){
for (j = 0; j < nv;j++){
if (fabs(v[j]) != 0.0){
rms += v[j];
nv_++;
}
}
rms = rms / nv_;
for (j = 0; j < nv_; j++){
RMS += (v[j] - rms)*(v[j] - rms);
}
RMS = sqrt(RMS/nv_); //�в�ľ�����;
if (RMS>2.5){
temp = fabs(v[0] - rms);
for (j = 1; j < nv_;j++){
if (temp <= fabs(v[j] - rms)){
temp = fabs(v[j] - rms);
kk = j;
}
else{
continue;
}
}
opt->exsats[rtk->sat_[kk] - 1] = 1;
}
rms = 0; RMS = 0; nv_ = 0; temp = 0; kk = 0;
//nv = nv - 1; //����һ������;
}
if (nv < NX) {
//sprintf(msg, "lack of valid sats ns=%d", nv);
break;
}
/* weight by variance */
for (j = 0; j < nv; j++) {
sig = sqrt(var[j]);
v[j] /= sig;
for (k = 0; k < NX; k++) H[k + j*NX] /= sig;
}
/* least square estimation */
if ((info=lsq(H,v,NX,nv,dx,Q))) {
//sprintf(msg,"lsq error info=%d",info);
break;
}
for (j=0;j<NX;j++)
x[j]+=dx[j];
if (norm(dx,NX)<1E-4) {
sol->type=0;
sol->time=timeadd(obs[0].time,-x[3]/CLIGHT);
sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */
sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */
sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */
sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */
for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0;
for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX];
sol->qr[3]=(float)Q[1]; /* cov xy */
sol->qr[4]=(float)Q[2+NX]; /* cov yz */
sol->qr[5]=(float)Q[2]; /* cov zx */
sol->ns=(unsigned char)ns;
sol->age=sol->ratio=0.0;
/* validate solution */
if ((stat = valsol(azel, vsat, n, opt, v, nv, NX, msg))) {
sol->stat = opt->sateph == EPHOPT_SBAS ? SOLQ_SBAS : SOLQ_SINGLE;
}
return stat;
}
}
return 0;
}
/* estimate receiver position ------------------------------------------------*/
static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
const double *vare, const int *svh, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
double *resp, char *msg)
{
double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig;
int i,j,k,info,stat,nv,ns;
trace(3,"estpos : n=%d\n",n);
v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1);
for (i=0;i<3;i++) x[i]=sol->rr[i];
fprintf(output, " Begin coordinates calculation in single point positioning mode\n");
fprintf(output, " Current coordinates: X = %f, Y = %f, Z = %f\n", x[0], x[1], x[2]);
for (i=0;i<MAXITR;i++) {
fprintf(output, " Begin iteration %i:\n", i);
/* pseudorange residuals */
nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,
&ns);
if (nv<NX) {
sprintf(msg,"lack of valid sats ns=%d",nv);
break;
}
/* weight by variance */
for (j=0;j<nv;j++) {
sig=sqrt(var[j]);
v[j]/=sig;
for (k=0;k<NX;k++) H[k+j*NX]/=sig;
}
fprintf(output, " Begin least-square estimation:\n");
fprintf(output, " ");
for(j = 0; j < NX; j++)
fprintf(output, " X[%i] = %f,", j, dx[j]);
fprintf(output, "\n");
fprintf(output, " ");
for(j = 0; j < nv; j++)
fprintf(output, " v[%i] = %f,", j, v[j]);
fprintf(output, "\n");
for(j = 0; j < nv; j++)
{
fprintf(output, " ");
for(k = 0; k < NX; k++)
fprintf(output, " H[%i][%i] = %f,", j, k, H[k + j * NX]);
fprintf(output, "\n");
}
/* least square estimation */
if ((info=lsq(H,v,NX,nv,dx,Q))) {
sprintf(msg,"lsq error info=%d",info);
break;
}
fprintf(output, " End least-square estimation\n");
fprintf(output, " Results of least square estimation: dx1 = %f, dx2 = %f, dx3 = %f, dx4 = %f, dx5 = %f, dx6 = %f, dx7 = %f\n", dx[0], dx[1], dx[2], dx[3], dx[4], dx[5], dx[6]);
for (j=0;j<NX;j++) x[j]+=dx[j];
fprintf(output, " New state of vector X: X[0] = %f, X[1] = %f, X[2] = %f, X[3] = %f, X[4] = %f, X[5] = %f, X[6] = %f\n", x[0], x[1], x[2], x[3], x[4], x[5], x[6]);
fprintf(output, " norm(dx) = %f\n", norm(dx, NX));
if (norm(dx,NX)<1E-4) {
sol->type=0;
sol->time=timeadd(obs[0].time,-x[3]/CLIGHT);
sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */
sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */
sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */
sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */
for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0;
for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX];
sol->qr[3]=(float)Q[1]; /* cov xy */
sol->qr[4]=(float)Q[2+NX]; /* cov yz */
sol->qr[5]=(float)Q[2]; /* cov zx */
sol->ns=(unsigned char)ns;
sol->age=sol->ratio=0.0;
/* validate solution */
if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) {
sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE;
}
free(v); free(H); free(var);
fprintf(output, " End of coordinates calculation\n");
return stat;
}
}
if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i);
free(v); free(H); free(var);
return 0;
}
