diff --git a/include/carvig.h b/include/carvig.h index a000382..0193071 100644 --- a/include/carvig.h +++ b/include/carvig.h @@ -77,7 +77,7 @@ extern "C"{ #define LAPACK #define TRACE /* trace information for debug */ #define TRACE_INS 1 /* trace ins updates information */ -#define TRACE_STDERR 0 /* trace information to stderr if set */ +#define TRACE_STDERR 1 /* trace information to stderr if set */ #define VIG_TRACE_MAT 1 /* trace matrix for debugs */ #define MAXBUFF 4096 /* size of line buffer */ #define MAXHIST 256 /* size of history buffer */ @@ -124,7 +124,7 @@ extern "C"{ #define EFACT_GLO 1.5 /* error factor: GLONASS */ #define EFACT_GAL 1.0 /* error factor: Galileo */ #define EFACT_QZS 1.0 /* error factor: QZSS */ -#define EFACT_CMP 1.0 /* error factor: BeiDou */ +#define EFACT_CMP 3.0 /* error factor: BeiDou */ #define EFACT_IRN 1.5 /* error factor: IRNSS */ #define EFACT_SBS 3.0 /* error factor: SBAS */ @@ -448,10 +448,10 @@ extern "C"{ #define SOLQ_PPP 6 /* solution status: PPP */ #define SOLQ_DR 7 /* solution status: dead reconing */ #define SOLQ_DOP 8 /* solution status: doppler measurement aid */ -#define SOLQ_INHERIT 9 /* solution status: ambiguity inherit fix status */ -#define SOLQ_ROUND 10 /* solution status: ambiguity round fix */ +#define SOLQ_WLFIX 9 /* solution status: WL ambiguity fix status */ +#define SOLQ_INHERIT_WL 10 /* solution status: WL ambiguity inherit fix */ #define SOLQ_VO 11 /* solution status: visual odometry status aid */ -#define SOLQ_GRTH 12 /* solution status: ground truth status */ +#define SOLQ_INHERIT 12 /* solution status: ambiguity inherit fix */ #define SOLQ_WAAS 13 /* solution status: solution calculated using corrections from an WAAS */ #define SOLQ_PROP 14 /* solution status: propagated by a kalman filter without new observations */ #define SOLQ_OMIN 15 /* solution status: OmniSTAR VBS position */ @@ -1976,7 +1976,8 @@ typedef struct { /* satellite status type */ gtime_t pt[2][NFREQ]; /* previous carrier-phase time */ double ph[2][NFREQ]; /* previous carrier-phase observable (cycle) */ double sdi[NFREQ]; /* single-differenced pseudorange observable by INS */ - double sdg[NFREQ]; /* single-differenced pseudorange observable by GNSS */ + double sdp[NFREQ]; /* code single-differenced pseudorange observable by GNSS */ + double sdc[NFREQ]; /* cycle single-differenced pseudorange observable by GNSS */ } ssat_t; typedef struct { /* ambiguity control type */ @@ -2000,7 +2001,7 @@ typedef struct { /* double-difference ambiguity type */ typedef struct { /* double-difference satellite */ int sat1,sat2; /* double difference satellite no. */ int f; /* frequency no. */ - int flag; + int flag; /* inherit flag (0: inherit, 1: no inherit) */ } ddsat_t; typedef struct { @@ -2027,7 +2028,7 @@ typedef struct { /* RTK control/result type */ ambc_t ambc[MAXSAT]; /* ambiguity control */ ssat_t ssat[MAXSAT]; /* satellite status */ insstate_t ins; /* ins states */ - amb_t bias; /* double-difference ambiguity list */ + amb_t bias; /* N1/N2 double-difference ambiguity list */ amb_t wlbias; /* WL double-difference ambiguity list */ ddsat_t sat[MAXSAT]; /* double difference satellite list */ } rtk_t; @@ -2407,6 +2408,9 @@ EXPORT int lsq (const double *A, const double *y, int n, int m, double *x, double *Q); EXPORT int filter(double *x, double *P, const double *H, const double *v, const double *R, int n, int m); +EXPORT void freekix(); +EXPORT int *getkix(); +EXPORT int getnix(); EXPORT int smoother(const double *xf, const double *Qf, const double *xb, const double *Qb, int n, double *xs, double *Qs); EXPORT void lsmooth3(double *in, double *out, int N); @@ -2887,7 +2891,7 @@ EXPORT int insinirtobs(rtksvr_t *svr,const obsd_t *obs,int n,const imud_t *imu); EXPORT int postpos(gtime_t ts, gtime_t te, double ti, double tu, const prcopt_t *popt, const solopt_t *sopt, const filopt_t *fopt, char **infile, int n, char *outfile, - const char *rov, const char *base); + const char *rov, const char *base,int port); /* precise point positioning -------------------------------------------------*/ EXPORT void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav); EXPORT int pppnx(const prcopt_t *opt); diff --git a/plot/plot.ini b/plot/plot.ini index 0f1325e..1cf947e 100644 --- a/plot/plot.ini +++ b/plot/plot.ini @@ -127,7 +127,7 @@ strmntphist_7= strmntphist_8= strmntphist_9= strpath1_0= -strpath1_1=:@localhost:52062/:: +strpath1_1=:@localhost:52090/:: strpath1_2= strpath2_0=:300:7:n:1:off strpath2_1=:@localhost:52779/:: diff --git a/src/ins-gnss/app/carvig-rtk.conf b/src/ins-gnss/app/carvig-rtk.conf index 6981e6d..008a9e2 100644 --- a/src/ins-gnss/app/carvig-rtk.conf +++ b/src/ins-gnss/app/carvig-rtk.conf @@ -6,7 +6,7 @@ console-solflag =1 # (0:off,1:std+2:age/ratio/ns) pos1-posmode =kinematic # (0:single,1:dgps,2:kinematic,3:static,4:movingbase,5:fixed,6:ppp-kine,7:ppp-static,8:ppp-fixed,9:ins-update,10:ins-loosely-coupled,11:ins-tightly-coupled,12:ins-vo-loosely-coupled,13:vo) pos1-frequency =l1+l2 # (1:l1,2:l1+l2,3:l1+l2+l5,4:l1+l5) pos1-soltype =forward # (0:forward,1:backward,2:combined) -pos1-elmask =10 # (deg) +pos1-elmask =15 # (deg) pos1-snrmask_r =off # (0:off,1:on) pos1-snrmask_b =off # (0:off,1:on) pos1-snrmask_L1 =0,0,0,0,0,0,0,0,0 @@ -24,7 +24,7 @@ pos1-posopt4 =on # (0:off,1:on) pos1-posopt5 =on # (0:off,1:on) pos1-posopt6 =off # (0:off,1:on) pos1-exclsats = # (prn ...) -pos1-navsys =1 # (1:gps+2:sbas+4:glo+8:gal+16:qzs+32:comp) +pos1-navsys =1 # (1:gps+2:sbas+4:glo+8:gal+16:qzs+32:comp) pos1-adjobs =1 # adjust observation data pos1-eps =1 # time internal between epochs pos1-gtfmt =1 # ground truth solution format @@ -38,7 +38,7 @@ pos2-arthres3 =0.1 pos2-arthres4 =0.05 pos2-arlockcnt =30 pos2-arelmask =0 # (deg) -pos2-arminfix =2 +pos2-arminfix =1 pos2-armaxiter =2 pos2-elmaskhold =0 # (deg) pos2-aroutcnt =30 @@ -125,8 +125,8 @@ file-tempdir = file-geexefile = file-solstatfile = file-tracefile =./trace.out -file-navfile =/media/sujinglan/Files/ins_gnss_data/20171107/第二组/被测2/171107161428.17n -file-bdsfile =/media/sujinglan/Files/ins_gnss_data/20171107/第二组/被测2/171107161428.17c +file-navfile =/home/sujinglan/ignav/example/data/3/rover/171107134322.17n +file-bdsfile =/home/sujinglan/ignav/example/data/3/rover/brdm3110.17p file-monodir = file-gtfile = @@ -137,11 +137,11 @@ inpstr4-type =file # (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,7:ntri inpstr5-type =file # (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,7:ntripcli,8:ftp,9:http) inpstr6-type =file # (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,7:ntripcli,8:ftp,9:http) inpstr7-type =file # (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,7:ntripcli,8:ftp,9:http) -inpstr1-path =/media/sujinglan/Files/ins_gnss_data/20171107/第二组/被测2/171107161428.17o -inpstr2-path =/media/sujinglan/Files/ins_gnss_data/20171107/base/5328K44285201711070000A.17O +inpstr1-path =/home/sujinglan/ignav/example/data/3/rover/171107134322-slp.17o +inpstr2-path =/home/sujinglan/ignav/example/data/3/base/5328K44285201711070000A.17O inpstr3-path = inpstr4-path = -inpstr5-path =/media/sujinglan/Files/ins_gnss_data/20171107/第二组/被测2/171107161428.imu +inpstr5-path =/home/sujinglan/ignav/example/data/3/rover/171107134322.imu inpstr6-path = inpstr7-path = inpstr1-format =rinex # (0:rtcm2,1:rtcm3,2:oem4,3:oem3,4:ubx,5:ss2,6:hemis,7:skytraq,8:gw10,9:javad,10:nvs,11:binex,12:rt17,13:sbf,14:cmr,15:tersus,18:sp3,19:rnxclk,20:sbas,21:nmea,22:gsof,23:ublox-evk-m8u,24:ublox-sol,25:m39,26:rinex,27:m39-mix,28:euroc-imu,29:euroc-img,30:karl-img,31:malaga-gnss,32:malaga-imu,33:malaga-img,34:oem6-sol,35:oem6-pose,36:oem6-raw) @@ -212,7 +212,7 @@ ins-imuformat =2 # imu measurement data format ins-imudecfmt =1 # imu measurement decode format ins-imucoors =2 # imu body coordinate frame ins-imuvalfmt =1 # imu measurement gyro data value format -ins-exprn =1 # use extend method to get process noise matrix +ins-exprn =0 # use extend method to get process noise matrix ins-exphi =0 # use precise system propagate matrix for ekf ins-exvm =0 # extend method to velocity matching for ins navigation initial ins-iisu =2 # initial ins state use rtk options (SOLQ_???) @@ -256,8 +256,8 @@ ins-unvma = # initial misalignment from v-frame to im ins-psd_gyro =5.72003802085e-09 # gyro noise PSD (rad^2/s) ins-psd_accl =2.33611111111e-07 # accelerometer noise PSD (m^2 s^-3) -ins-psd_ba =1E-10 # accelerometer bias random walk PSD (m^2 s^-5) -ins-psd_bg =1E-12 # gyro bias random walk PSD (rad^2 s^-3) +ins-psd_ba =1E-07 # accelerometer bias random walk PSD (m^2 s^-5) +ins-psd_bg =2E-12 # gyro bias random walk PSD (rad^2 s^-3) ins-psd_dt = # time synchronization error noise PSD ins-psd_sg = # residual scale factors of gyroscopes noise PSD ins-psd_sa = # residual scale factors of accl noise PSD diff --git a/src/ins-gnss/app/carvig-tc.conf b/src/ins-gnss/app/carvig-tc.conf index 1c81477..7fc4bac 100644 --- a/src/ins-gnss/app/carvig-tc.conf +++ b/src/ins-gnss/app/carvig-tc.conf @@ -3,7 +3,7 @@ console-timetype =gpst # (0:gpst,1:utc,2:jst,3:tow) console-soltype =dms # (0:dms,1:deg,2:xyz,3:enu,4:pyl) console-solflag =1 # (0:off,1:std+2:age/ratio/ns) -pos1-posmode =kinematic # (0:single,1:dgps,2:kinematic,3:static,4:movingbase,5:fixed,6:ppp-kine,7:ppp-static,8:ppp-fixed,9:ins-update,10:ins-loosely-coupled,11:ins-tightly-coupled,12:ins-vo-loosely-coupled,13:vo) +pos1-posmode =ins-tightly-coupled # (0:single,1:dgps,2:kinematic,3:static,4:movingbase,5:fixed,6:ppp-kine,7:ppp-static,8:ppp-fixed,9:ins-update,10:ins-loosely-coupled,11:ins-tightly-coupled,12:ins-vo-loosely-coupled,13:vo) pos1-frequency =l1+l2 # (1:l1,2:l1+l2,3:l1+l2+l5,4:l1+l5) pos1-soltype =forward # (0:forward,1:backward,2:combined) pos1-elmask =15 # (deg) @@ -50,7 +50,7 @@ pos2-rejgdop =30 pos2-niter =2 pos2-baselen =0 # (m) pos2-basesig =0 # (m) -out-solformat =xyz # (0:llh,1:xyz,2:enu,3:nmea,4:stat,5:gsif,6:ins,7:vo) +out-solformat =ins # (0:llh,1:xyz,2:enu,3:nmea,4:stat,5:gsif,6:ins,7:vo) out-outhead =on # (0:off,1:on) out-outopt =on # (0:off,1:on) out-timesys =gpst # (0:gpst,1:utc,2:jst) @@ -126,7 +126,7 @@ file-geexefile = file-solstatfile = file-tracefile =./trace.out file-navfile =/home/sujinglan/ignav/example/data/3/rover/171107134322.17n -file-bdsfile =/home/sujinglan/ignav/example/data/3/rover/171107161428.17c +file-bdsfile =/home/sujinglan/ignav/example/data/3/rover/brdm3110.17p file-monodir = file-gtfile = @@ -137,7 +137,7 @@ inpstr4-type =file # (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,7:ntri inpstr5-type =file # (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,7:ntripcli,8:ftp,9:http) inpstr6-type =file # (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,7:ntripcli,8:ftp,9:http) inpstr7-type =file # (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,7:ntripcli,8:ftp,9:http) -inpstr1-path =/home/sujinglan/ignav/example/data/3/rover/171107134322.17o +inpstr1-path =/home/sujinglan/ignav/example/data/3/rover/171107134322-slp.17o inpstr2-path =/home/sujinglan/ignav/example/data/3/base/5328K44285201711070000A.17O inpstr3-path = inpstr4-path = @@ -256,8 +256,8 @@ ins-unvma = # initial misalignment from v-frame to im ins-psd_gyro =5.72003802085e-09 # gyro noise PSD (rad^2/s) ins-psd_accl =2.33611111111e-07 # accelerometer noise PSD (m^2 s^-3) -ins-psd_ba =1E-10 # accelerometer bias random walk PSD (m^2 s^-5) -ins-psd_bg =1E-12 # gyro bias random walk PSD (rad^2 s^-3) +ins-psd_ba =1E-07 # accelerometer bias random walk PSD (m^2 s^-5) +ins-psd_bg =2E-12 # gyro bias random walk PSD (rad^2 s^-3) ins-psd_dt = # time synchronization error noise PSD ins-psd_sg = # residual scale factors of gyroscopes noise PSD ins-psd_sa = # residual scale factors of accl noise PSD diff --git a/src/ins-gnss/app/rnx2rtkp.cc b/src/ins-gnss/app/rnx2rtkp.cc index 9a28f00..8ed814e 100644 --- a/src/ins-gnss/app/rnx2rtkp.cc +++ b/src/ins-gnss/app/rnx2rtkp.cc @@ -110,12 +110,13 @@ int main(int argc, char **argv) filopt_t filopt={""}; gtime_t ts={0},te={0}; double tint=0.0; - int i,n,ret; + int i,n,ret,port=0; char *infile[MAXFILE],*outfile="",file[1024]; for (i=1;inx,info=1,nu,nr,flag; - double *P,dt; + double dt; const insopt_t* insopt=&opt->insopt; if (imu==NULL) return 0; @@ -184,7 +184,6 @@ extern int tcigpos(const prcopt_t *opt,const obsd_t *obs,int n,const nav_t *nav, trace(2,"ins mechanization update fail\n"); return 0; } - P=zeros(nx,nx); propinss(ins,insopt,ins->dt,ins->x,ins->P); /* check variance of estimated position */ @@ -196,6 +195,8 @@ extern int tcigpos(const prcopt_t *opt,const obsd_t *obs,int n,const nav_t *nav, info=1; } else { + for (i=0;i<6;i++) rtk->sol.pqr[i]=rtk->sol.qr[i]; + rtk->sol.pstat=rtk->sol.stat; ins->gstat=SOLQ_NONE; ins->ns=0; #if REBOOT @@ -257,6 +258,6 @@ extern int tcigpos(const prcopt_t *opt,const obsd_t *obs,int n,const nav_t *nav, } } exit: - free(P); return info; + return info; } diff --git a/src/ins-gnss/pntpos.cc b/src/ins-gnss/pntpos.cc index 0cb4bd4..476dcfe 100644 --- a/src/ins-gnss/pntpos.cc +++ b/src/ins-gnss/pntpos.cc @@ -160,7 +160,7 @@ extern int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos, trace(4,"ionocorr: time=%s opt=%d sat=%2d pos=%.3f %.3f azel=%.3f %.3f\n", time_str(time,3),ionoopt,sat,pos[0]*R2D,pos[1]*R2D,azel[0]*R2D, azel[1]*R2D); - + /* broadcast model */ if (ionoopt==IONOOPT_BRDC) { *ion=ionmodel(time,nav->ion_gps,pos,azel); @@ -203,10 +203,11 @@ extern int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos, extern int tropcorr(gtime_t time, const nav_t *nav, const double *pos, const double *azel, int tropopt, double *trp, double *var) { + trace(4,"tropcorr: time=%s opt=%d pos=%.3f %.3f azel=%.3f %.3f\n", time_str(time,3),tropopt,pos[0]*R2D,pos[1]*R2D,azel[0]*R2D, azel[1]*R2D); - + /* saastamoinen model */ if (tropopt==TROPOPT_SAAS||tropopt==TROPOPT_EST||tropopt==TROPOPT_ESTG) { *trp=tropmodel(time,pos,azel,REL_HUMI); @@ -816,9 +817,6 @@ extern int pntpos(const obsd_t *obs, int n, const nav_t *nav,const prcopt_t *opt return 0; } trace(3,"pntpos : tobs=%s n=%d\n",time_str(obs[0].time,3),n); - - trace(4,"obs=\n"); traceobs(4,obs,n); - trace(5,"nav=\n"); tracenav(5,nav); sol->time=obs[0].time; msg[0]='\0'; diff --git a/src/ins-gnss/postpos.cc b/src/ins-gnss/postpos.cc index e9822e0..f922898 100644 --- a/src/ins-gnss/postpos.cc +++ b/src/ins-gnss/postpos.cc @@ -47,6 +47,7 @@ #define MAXPRCDAYS 100 /* max days of continuous processing */ #define MAXINFILE 1000 /* max number of input files */ +#define INTKEEPALIVE 1000 /* keep alive interval (ms) */ /* constants/global variables ------------------------------------------------*/ @@ -64,6 +65,10 @@ static int isbs =0; /* current sbas message index */ static int ilex =0; /* current lex message index */ static int revs =0; /* analysis direction (0:forward,1:backward) */ static int aborts=0; /* abort status */ +static int mport =0; /* port of monitor */ +static int keepalive=1; /* keep alive flag */ +static int timeout =10000; /* timeout time (ms) */ +static int reconnect=10000; /* reconnect interval (ms) */ static sol_t *solf; /* forward solutions */ static sol_t *solb; /* backward solutions */ static double *rbf; /* forward base positions */ @@ -76,7 +81,36 @@ static char rtcm_file[1024]=""; /* rtcm data file */ static char rtcm_path[1024]=""; /* rtcm data path */ static rtcm_t rtcm; /* rtcm control struct */ static FILE *fp_rtcm=NULL; /* rtcm data file pointer */ +static stream_t moni={0}; /* monitor stream */ +/* thread to send keep alive for monitor port --------------------------------*/ +static void *sendkeepalive(void *arg) +{ + trace(3,"sendkeepalive: start\n"); + + while (keepalive) { + strwrite(&moni,(unsigned char *)"\r",1); + sleepms(INTKEEPALIVE); + } + trace(3,"sendkeepalive: stop\n"); + return NULL; +} +/* open monitor port ---------------------------------------------------------*/ +static int openmoni(int port) +{ + pthread_t thread; + char path[64]; + + trace(3,"openmomi: port=%d\n",port); + + sprintf(path,":%d",port); + + if (!stropen(&moni,STR_TCPSVR,STR_MODE_RW,path)) return 0; + strsettimeout(&moni,timeout,reconnect); + keepalive=1; + pthread_create(&thread,NULL,sendkeepalive,NULL); + return 1; +} /* show message and check break ----------------------------------------------*/ static int checkbrk(const char *format, ...) { @@ -91,6 +125,19 @@ static int checkbrk(const char *format, ...) else if (*proc_base) sprintf(p," (%s)",proc_base); return showmsg(buff); } +/* close monitor port---------------------------------------------------------*/ +static int close_moni(stream_t *moni) +{ + trace(3,"closemoni:\n"); + keepalive=0; + + /* send disconnect message */ + strwrite(moni,(unsigned char *)MSG_DISCONN,strlen(MSG_DISCONN)); + + /* wait fin from clients */ + sleepms(1000); + strclose(moni); +} /* output reference position -------------------------------------------------*/ static void outrpos(FILE *fp, const double *r, const solopt_t *opt) { @@ -320,6 +367,19 @@ static int inputobs(obsd_t *obs, int solq, const prcopt_t *popt) } return n; } +/* write solution status output stream ---------------------------------------*/ +static int wrt_solution(rtk_t *rtk,const solopt_t *solopt) +{ + unsigned char buff[1024]; + int n=0; + + /* output solution to monitor */ + if (moni.port) { + n=outsols(buff,&rtk->sol,rtk->rb,solopt,&rtk->ins,&rtk->opt.insopt,1); + strwrite(&moni,buff,n); + } + return n; +} /* process positioning -------------------------------------------------------*/ static void procpos(FILE *fp, const prcopt_t *popt, const solopt_t *sopt, int mode) @@ -366,6 +426,8 @@ static void procpos(FILE *fp, const prcopt_t *popt, const solopt_t *sopt, if (mode==0) { /* forward/backward */ if (!solstatic) { outsol(fp,&rtk.sol,rtk.rb,sopt,NULL,&popt->insopt); + + wrt_solution(&rtk,sopt); } else if (time.time==0||pri[rtk.sol.stat]<=pri[sol.stat]) { sol=rtk.sol; @@ -1194,6 +1256,7 @@ static int execses_b(gtime_t ts, gtime_t te, double ti, const prcopt_t *popt, * char *outfile I output file ("":stdout, see below) * char *rov I rover id list (separated by " ") * char *base I base station id list (separated by " ") +* int port I port of monitor * return : status (0:ok,0>:error,1:aborted) * notes : input files should contain observation data, navigation data, precise * ephemeris/clock (optional), sbas log file (optional), ssr message @@ -1228,7 +1291,7 @@ static int execses_b(gtime_t ts, gtime_t te, double ti, const prcopt_t *popt, extern int postpos(gtime_t ts, gtime_t te, double ti, double tu, const prcopt_t *popt, const solopt_t *sopt, const filopt_t *fopt, char **infile, int n, char *outfile, - const char *rov, const char *base) + const char *rov, const char *base,int port) { gtime_t tts,tte,ttte; filopt_t fopt_=*fopt; @@ -1237,7 +1300,13 @@ extern int postpos(gtime_t ts, gtime_t te, double ti, double tu, char *ifile[MAXINFILE],ofile[1024],*ext; trace(3,"postpos : ti=%.0f tu=%.0f n=%d outfile=%s\n",ti,tu,n,outfile); - + + mport=port; + + /* open monitor */ + if (port&&!openmoni(mport)) { + fprintf(stderr,"open monitor fail\n"); + } /* open processing session */ if (!openses(popt,sopt,fopt,&navs,&pcvss,&pcvsr)) return -1; @@ -1334,6 +1403,8 @@ extern int postpos(gtime_t ts, gtime_t te, double ti, double tu, } /* close processing session */ closeses(&navs,&pcvss,&pcvsr); - + + /* close monitor */ + if (mport) close_moni(&moni); return stat; } diff --git a/src/ins-gnss/rcv/ground-truth.cc b/src/ins-gnss/rcv/ground-truth.cc index 9c5919f..f683b2c 100644 --- a/src/ins-gnss/rcv/ground-truth.cc +++ b/src/ins-gnss/rcv/ground-truth.cc @@ -42,7 +42,7 @@ static int readkarl(FILE *fp,solbuf_t *solbuf) stamp=stamp*1E-9; sol.time.sec =stamp-(time_t)stamp; sol.time.time=(time_t)stamp; - sol.stat=SOLQ_GRTH; + sol.stat=SOLQ_NONE; pos[0]*=D2R; pos[1]*=D2R; pos2ecef(pos,sol.rr); diff --git a/src/ins-gnss/rtkcmn.cc b/src/ins-gnss/rtkcmn.cc index bc9e120..942d4d0 100644 --- a/src/ins-gnss/rtkcmn.cc +++ b/src/ins-gnss/rtkcmn.cc @@ -1556,8 +1556,6 @@ extern int filter(double *x, double *P, const double *H, const double *v, double *x_,*xp_,*P_,*Pp_,*H_; int i,j,k,info,*ix; - for (i=0;i0.0)&&x[i]!=DISFLAG) ix[k++]=i; } @@ -3572,6 +3570,7 @@ extern void trace(int level, const char *format, ...) if (level<=1) { va_start(ap,format); vfprintf(stderr,format,ap); va_end(ap); } + if (level==4) return; #if TRACE_STDERR fp_trace=stderr; #endif @@ -3643,9 +3642,9 @@ extern void traceobs(int level, const obsd_t *obs, int n) for (i=0;i #include -#include /* constants/macros ----------------------------------------------------------*/ #define VAR_POS SQR(30.0) /* initial variance of receiver pos (m^2) */ @@ -60,6 +59,7 @@ #define THRES_HOLDAMB 3.0 /* threshold of hold ambiguity */ #define THRES_MW_JUMP 5.0 /* threshold of MW cycle slip detect*/ #define MINFAILC 30 /* min counts of solution valid fail */ +#define MINFIXC 4 /* min fix count for fix abmguity */ #define UPDNEWSAT 1 /* update new satellites after update last epoch satellites */ #define NOINSJACO 0 /* no add jacobians of ins states to ekf filter */ #define DETECT_OUTLIER 1 /* detect outlier for double-differenced measurement data */ @@ -67,7 +67,7 @@ #define THRES_INHERIT_TIME 1.5 /* threshold of ambiguity inherit */ #define THRES_INHERIT_BIAS 1.0 /* threshold of ambiguity inherit */ -#define INHERIT_AMB 1 +#define INHERIT_AMB 0 /* ambiguity inherit option */ #define TTOL_MOVEB (1.0+2*DTTOL) /* time sync tolerance for moving-baseline (s) */ @@ -505,16 +505,18 @@ static double varerr(int sat, int sys, double el, double bl, double dt, int f, a=opt->exterr.cerr[i][ (f-nf)*2]; b=opt->exterr.cerr[i][1+(f-nf)*2]; if (sys==SYS_SBS) {a*=EFACT_SBS; b*=EFACT_SBS;} + if (sys==SYS_CMP) {a*=EFACT_CMP; b*=EFACT_CMP;} } else if (fexterr.ena[1]) { /* phase */ a=opt->exterr.perr[i][ f*2]; b=opt->exterr.perr[i][1+f*2]; if (sys==SYS_SBS) {a*=EFACT_SBS; b*=EFACT_SBS;} + if (sys==SYS_CMP) {a*=EFACT_CMP; b*=EFACT_CMP;} } else { /* normal error model */ if (f>=nf) fact=opt->eratio[f-nf]; if (fact<=0.0) fact=opt->eratio[0]; - fact*=sys==SYS_GLO?EFACT_GLO:(sys==SYS_SBS?EFACT_SBS:EFACT_GPS); + fact*=sys==SYS_GLO?EFACT_GLO:(sys==SYS_SBS?EFACT_SBS:(sys==SYS_CMP?EFACT_CMP:EFACT_GPS)); a=fact*opt->err[1]; b=fact*opt->err[2]; } @@ -976,7 +978,7 @@ static void udbias(rtk_t *rtk, double tt, const obsd_t *obs, const int *sat, con } else if (reset&&x[ib]!=0.0) { tc? - insinitx(&rtk->ins,0.0,SQR(rtk->opt.std[0]),ib): + insinitx(&rtk->ins,0.0,0.0,ib): initx(rtk,0.0,SQR(rtk->opt.std[0]),ib); trace(3,"obs outage counter overflow (sat=%3d L%d n=%d)\n",i,f+1,rtk->ssat[i-1].outc[f]); rtk->ssat[i-1].outc[f]=0; @@ -1009,7 +1011,8 @@ static void udbias(rtk_t *rtk, double tt, const obsd_t *obs, const int *sat, con pr=sdobs(obs,iu[i],ir[i],f+NFREQ); rtk->ssat[sat[i]-1].sdi[f]=sdobspins(rtk,obs,rs,iu[i],ir[i],f+NFREQ); - rtk->ssat[sat[i]-1].sdg[f]=pr; + rtk->ssat[sat[i]-1].sdp[f]=pr; + rtk->ssat[sat[i]-1].sdc[f]=cp; lami=nav->lam[sat[i]-1][f]; if (cp==0.0||pr==0.0||lami<=0.0) continue; @@ -1205,7 +1208,7 @@ static int zdres(int base, const obsd_t *obs, int n, const double *rs, obs[i].sat,rs[i*6],rs[1+i*6],rs[2+i*6],dts[i*2],azel[i*2]*R2D, azel[1+i*2]*R2D); } - trace(4,"y=\n"); tracemat(4,y,nf*2,n,15,6); + trace(3,"y=\n"); tracemat(3,y,nf*2,n,15,6); return 1; } @@ -1421,14 +1424,14 @@ static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt,cons double *e, double *azel, const int *iu, const int *ir, int ns, double *v, double *H, double *R, int *vflg,const double *re, double *Ri_,double *Rj_,int *nb_,int *b_, - int refsat[NUMSYS][2*NFREQ]) + int refsat[NUMSYS][2*NFREQ],int usedualfrq) { prcopt_t *opt=&rtk->opt; insopt_t *insopt=&opt->insopt; double bl,dr[3],posu[3],posr[3],didxi,didxj,*im,*vc,ddi,ddg,factor=1.0; double *tropr,*tropu,*dtdxr,*dtdxu,*Ri,*Rj,lami,lamj,fi,fj,df,*Hi=NULL,rr[3]; double dp[3]={0},da[3]={0},dl[3]={0},S[9],dap[3]; - int i,j,k,m,f,ff,nv=0,nb[NFREQ*4*2+2]={0},b=0,sysi,sysj,nf=NF(opt),tc,nx; + int i,i1,j,k,m,f,ff,nv=0,nb[NFREQ*4*2+2]={0},b=0,sysi,sysj,nf=NF(opt),tc,nx; int ii,ij,flag=0; trace(3,"ddres : dt=%.1f ns=%d\n",dt,ns); @@ -1474,26 +1477,33 @@ static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt,cons for (f=(opt->mode>PMODE_DGPS&&opt->modessat[sat[j]-1].sys; if (!test_sys(sysi,m)) continue; if (!validobs(iu[j],ir[j],f,nf,y)) continue; + if (fssat[sat[j]-1].slip[f]) { - continue; - } + if (rtk->ssat[sat[j]-1].slip[f]) continue; + } + if (i<0||azel[1+iu[j]*2]>=azel[1+iu[i]*2]) { + i1=i; i=j; } - if (i<0||azel[1+iu[j]*2]>=azel[1+iu[i]*2]) i=j; } if (i<0) continue; - + if (i1>=0&&rtk->ssat[sat[i1]-1].lock[f]+rtk->opt.minlock> + rtk->ssat[sat[i]-1].lock[f]+rtk->opt.minlock) { + i=i1; + } /* double difference satellite */ - if (refsat) refsat[m][f]=sat[i]; + if (refsat) { + /* set double difference reference satelite */ + refsat[m][f]=sat[i]; + } /* make double difference */ for (j=0;jssat[sat[j]-1].slip[f]) { @@ -1528,8 +1538,8 @@ static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt,cons /* partial derivatives by rover position */ if (H) { if (!tc) for (k=0;k<3;k++) { - Hi[k]=-e[k+iu[i]*3]+e[k+iu[j]*3]; - } + Hi[k]=-e[k+iu[i]*3]+e[k+iu[j]*3]; + } else { /* partial derivations by ins position */ jacob_dd_dp(&rtk->ins,&e[iu[i]*3],&e[iu[j]*3],dp); @@ -1544,8 +1554,8 @@ static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt,cons #if UPD_IN_EULER jacobian_prot_pang(rtk->ins.Cbe,S); - matcpy(dap,da,1,3); - matmul("NN",1,3,3,1.0,dap,S,0.0,da); + matcpy(dap,da,1,3); + matmul("NN",1,3,3,1.0,dap,S,0.0,da); #endif Hi[xiA(insopt)+0]=da[0]; Hi[xiA(insopt)+1]=da[1]; @@ -1619,8 +1629,10 @@ static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt,cons if (f>=nf&&tc) { ddi=rtk->ssat[sat[i]-1].sdi[f%nf]-rtk->ssat[sat[j]-1].sdi[f%nf]; - ddg=rtk->ssat[sat[i]-1].sdg[f%nf]-rtk->ssat[sat[j]-1].sdg[f%nf]; - if (fabs(ddi-ddg)>1.0) continue; + ddg=rtk->ssat[sat[i]-1].sdp[f%nf]-rtk->ssat[sat[j]-1].sdp[f%nf]; + if (fabs(ddi-ddg)>1.0) { + factor*=5.0; + } } /* test innovation */ if (opt->maxinno>0.0&&fabs(v[nv])>opt->maxinno) { @@ -1628,9 +1640,9 @@ static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt,cons rtk->ssat[sat[i]-1].rejc[f]++; rtk->ssat[sat[j]-1].rejc[f]++; } - errmsg(rtk,"outlier rejected (sat=%3d-%3d %s%d v=%.3f)\n", - sat[i],sat[j],f=nf) { rtk->ssat[sat[i]-1].vsatc[f-nf]=rtk->ssat[sat[j]-1].vsatc[f-nf]=1; } - trace(4,"sat=%3d-%3d %s%d v=%13.3f R=%8.6f %8.6f\n", + trace(3,"sat=%3d-%3d %s%d v=%13.3f R=%8.6f %8.6f\n", sat[i],sat[j],fssat[sat[j]-1].news[f]=1; /* disable double difference measurement */ - Rj[nv]=SQR(30.0); + Rj[nv]*=10.0; } #endif Rj[nv]*=factor; @@ -1676,7 +1688,7 @@ static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt,cons static double s0; /* detect outlier by L1/L2 phase double difference residual */ - if (nf>=2&&opt->mode>PMODE_DGPS) { + if (nv&&nf>=2&&opt->mode>PMODE_DGPS) { double *vv=mat(ns,1),avv=0.0,v0=0.0; int l,*s=imat(ns,1); @@ -1686,8 +1698,8 @@ static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt,cons if (!rtk->ssat[sat[i]-1].vsat[0]) continue; /* exclude reference satellite */ - if (rtk->refsat[sysind(rtk->ssat[sat[i]-1].sys)][0]==sat[i]) continue; - if (rtk->refsat[sysind(rtk->ssat[sat[i]-1].sys)][1]==sat[i]) continue; + if (refsat[sysind(rtk->ssat[sat[i]-1].sys)][0]==sat[i]) continue; + if (refsat[sysind(rtk->ssat[sat[i]-1].sys)][1]==sat[i]) continue; /* L1-L2 */ j=rtk->ssat[sat[i]-1].index[0]; @@ -1720,6 +1732,7 @@ static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt,cons for (f=0;fssat[sat[s[i]]-1].vsat[f]=0; + rtk->ssat[sat[s[i]]-1].lock[f]++; } /* outlier flag */ flag=0; @@ -1730,31 +1743,45 @@ static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt,cons free(s); free(vv); } /* code double-difference measurement outliers detect */ - for (s0=0.0,k=0,i=0;i>4)&0xF)==0) continue; - vc[k++]=v[i]; - s0+=v[i]; - } - if (k>2) { - s0/=k; - for (i=0;i>4)&0xF)==0) continue; - if (fabs(vc[k])/SQRT(s0)>=r1) { - /* disable this satellite */ - Rj[i]=SQR(100.0); - } - else if (fabs(vc[k])/SQRT(s0)>=r0) { + /* sat no. */ + sat1=(vflg[i]>>16)&0xFF; + sat2=(vflg[i]>> 8)&0xFF; - /* degrade this satellite */ - Rj[i]=SQR(10.0); + /* test navi system */ + if (!test_sys(rtk->ssat[sat1-1].sys,m)) continue; + if (!test_sys(rtk->ssat[sat2-1].sys,m)) continue; + + vc[k]=v[i]; + s0+=v[i]; + ind[k++]=i; + } + if (k>2) { + s0/=k; + for (i=0;i=r1) { + + /* disable this satellite */ + Rj[ind[i]]=SQR(100.0); + } + else if (fabs(vc[i])/SQRT(s0)>=r0) { + + /* degrade this satellite */ + Rj[ind[i]]=SQR(10.0); + } } - k++; } + free(ind); } #endif /* baseline length constraint for moving baseline */ @@ -1818,24 +1845,24 @@ static double intpres(gtime_t time, const obsd_t *obs, int n, const nav_t *nav, static int ddmat_WL(int na,int nb,const ddsat_t *ddsat,double *D, int *index,ddsat_t *wlsat) { - int i,j,k,*flag; + int i,j,k,*flag,ny=na+nb; trace(3,"ddmat_WL:\n"); flag=imat(nb,1); for (i=0;inx,na=rtk->na; + static unsigned char fixflag[NFREQ]={0}; double *x=rtk->x; trace(3,"ddmat :\n"); @@ -1922,7 +1953,7 @@ static int ddmat(rtk_t *rtk, double *D,ddsat_t *ddsat,const int *vflg,int nv,int if (tc) { x=rtk->ins.x; } - for (i=0;issat[i].fix[j]=0; + for (i=0;issat[i].fix,fixflag,sizeof(unsigned char)*NFREQ); for (i=0;issat[sat1-1].fix[f]=2; -#if UPDNEWSAT - /* no fix new double difference ambiguity */ - if (rtk->ssat[sat2-1].news[f]) continue; -#endif + /* check double difference ambiguity value */ if (x[j]==0.0||x[k]==0.0) continue; + /* slicp cycle */ + if (rtk->ssat[sat2-1].slip[f]) continue; + if ((rtk->ssat[sat2-1].lock[f]>=0||flag)&& - rtk->ssat[sat2-1].vsat[f]&&!(rtk->ssat[j-k].slip[f]&2)&& + rtk->ssat[sat2-1].vsat[f]&& rtk->ssat[sat2-1].azel[1]>=rtk->opt.elmaskar) { D[k+(na+nb)*nx]= 1.0; @@ -1974,6 +2005,18 @@ static int ddmat(rtk_t *rtk, double *D,ddsat_t *ddsat,const int *vflg,int nv,int } trace(5,"D=\n"); tracemat(5,D,nx,na+nb,2,0); + + trace(3,"fix double difference satelites:\n"); + for (i=0;issat[ddsat[i].sat1-1].lock[0],rtk->ssat[ddsat[i].sat1-1].lock[1], + rtk->ssat[ddsat[i].sat2-1].lock[0],rtk->ssat[ddsat[i].sat2-1].lock[1], + rtk->ssat[ddsat[i].sat1-1].azel[1]*R2D, + rtk->ssat[ddsat[i].sat2-1].azel[1]*R2D, + rtk->ssat[ddsat[i].sat1-1].slip[0], + rtk->ssat[ddsat[i].sat2-1].slip[0]); + } return nb; } /* restore single-differenced ambiguity --------------------------------------*/ @@ -2010,6 +2053,39 @@ static void restamb(rtk_t *rtk, const double *bias, ddsat_t *ddsat, xa[ib2]=xa[ib1]-bias[i]; } } +/* restore single-differenced ambiguity --------------------------------------*/ +static void restambWL(rtk_t *rtk,const int *index,int nw,ddsat_t *ddsat, + int nb,const double *wlbias,const double *bias, + double *xs) +{ + double *y=mat(nb,1); + int i,sat1,sat2,f,ib1,ib2,tc; + + trace(3,"restambWL: nw=%d\n",nw); + + tc=rtk->opt.mode==PMODE_INS_TGNSS; + + matcpy(y,bias,1,nb); + for (i=0;iopt.insopt,sat1,f):IB(sat1,f,&rtk->opt); + ib2=tc?xiBs(&rtk->opt.insopt,sat2,f):IB(sat2,f,&rtk->opt); + + /* restore single-differenced ambiguity */ + xs[ib2]=xs[ib1]-y[i]; + } + free(y); +} /* hold integer ambiguity ----------------------------------------------------*/ static void holdamb(rtk_t *rtk, insstate_t *ins, const double *xa,const ddsat_t *ddsat, int ns) @@ -2018,7 +2094,7 @@ static void holdamb(rtk_t *rtk, insstate_t *ins, const double *xa,const ddsat_t int i,f,info; int nb,nv=0; int sat1,sat2,ib1,ib2,tc,nx; - ddamb_t *pamb; + ddamb_t *pamb=NULL; trace(3,"holdamb :ns=%d\n",ns); @@ -2052,7 +2128,7 @@ static void holdamb(rtk_t *rtk, insstate_t *ins, const double *xa,const ddsat_t /* constraint to fixed ambiguity */ v[nv]=(xa[ib1]-xa[ib2])-(x[ib1]-x[ib2]); -#if 1 +#if 0 /* get double difference ambiguity of precious epoch */ pamb=getddamb(&rtk->bias,sat1,sat2,f); if (!pamb) { @@ -2140,7 +2216,7 @@ static int inheritambwl(rtk_t *rtk,const ddsat_t *wlsat,const double *wl, for (i=0;iwlbias,wlsat[i].sat1,wlsat[i].sat2,wlsat[i].f))==NULL) continue; - if (pamb->ratioopt.thresar[0]*3.0) continue; + if (pamb->ratioopt.thresar[0]*1.5) continue; if (timediff(rtk->sol.time,pamb->time)>THRES_INHERIT_TIME) continue; if (fabs(wl[i]-pamb->bias)>THRES_INHERIT_BIAS) continue; @@ -2151,6 +2227,8 @@ static int inheritambwl(rtk_t *rtk,const ddsat_t *wlsat,const double *wl, if (k>=2) { s[1]=999.0; s[0]=1.00; + rtk->sol.stat=SOLQ_INHERIT_WL; + trace(3,"inherit ambiguity=\n"); tracemat(3,b,1,nw,12,6); return 1; @@ -2160,7 +2238,8 @@ static int inheritambwl(rtk_t *rtk,const ddsat_t *wlsat,const double *wl, } /* resolve WL integer ambiguity by LAMBDA------------------------------------*/ static int resamb_WL(rtk_t *rtk, double *Qy, double *y, int ny, int *index,const double *D, - int nw, ddsat_t *wlsat) + int nw, ddsat_t *ddsat,const ddsat_t *wlsat, + double *dxwl,double *Qywl) { int i,j,k,info=0,na,tc,inherit=0; double *Qw,*wl,*b,*v,*R,*r,*H,s[2]; @@ -2180,6 +2259,10 @@ static int resamb_WL(rtk_t *rtk, double *Qy, double *y, int ny, int *index,const /* covariance matrix of WL ambiguity */ matmul33("TNN",D,Qy,D,nw,ny,ny,nw,Qw); + + trace(3,"Qw=\n"); + tracemat(3,Qw,nw,nw,12,6); + for (i=0;isol.wlratio=s[0]>0?(float)(s[1]/s[0]):0.0f; if (rtk->sol.wlratio>999.9f) { @@ -2219,16 +2303,16 @@ static int resamb_WL(rtk_t *rtk, double *Qy, double *y, int ny, int *index,const info=0; } else { - /* WL fix to update rover position */ - if (tc) clp(&rtk->ins,&rtk->opt.insopt,y); - else { - for (i=0;ix[i]=y[i]; - for (i=0;iPa[i+j*na]=Qy[i+j*ny]; - } - } + /* output solution */ + matcpy(dxwl,y , 1,na); + matcpy(Qywl,Qy,ny,ny); + + rtk->sol.stat=SOLQ_WLFIX; /* fix ok */ info=1; + + /* restore single ambiguity */ + restambWL(rtk,index,nw,ddsat,ny-na,b,y+na,dxwl); } /* store WL ambiguity */ if (info) { @@ -2264,7 +2348,6 @@ static int inheritamb(rtk_t *rtk, ddsat_t *ddsat,const double *y,const double *Q for (i=0;ibias,ddsat[i].sat1,ddsat[i].sat2,ddsat[i].f))==NULL) continue; - if (pamb->ratioopt.thresar[0]*10.0) continue; if (timediff(rtk->sol.time,pamb->time)>THRES_INHERIT_TIME) continue; if (fabs(y[i]-pamb->bias)>THRES_INHERIT_BIAS) continue; @@ -2276,6 +2359,8 @@ static int inheritamb(rtk_t *rtk, ddsat_t *ddsat,const double *y,const double *Q if (k>=3) { s[1]=99.9; s[0]=1.00; + rtk->sol.stat=SOLQ_INHERIT; + trace(3,"inherit ambiguity=\n"); tracemat(3,b,1,nb,12,6); return 1; @@ -2285,7 +2370,8 @@ static int inheritamb(rtk_t *rtk, ddsat_t *ddsat,const double *y,const double *Q } /* resolve integer ambiguity by LAMBDA --------------------------------------*/ static int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa, ddsat_t *ddsat,int *namb, - const int *vflg,int nv) + double *dxwl,double *Qywl,const int *vflg,int nv,int fixnolock, + int *nny) { prcopt_t *opt=&rtk->opt; insstate_t *ins=&rtk->ins; @@ -2320,24 +2406,25 @@ static int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa, ddsat_t *ddsat,in tc?xb=rtk->ins.xb:xb=rtk->xa; /* tc-mode */ - if (opt->mode==PMODE_INS_TGNSS) {nx=ins->nx; na=ins->nb;} - + if (opt->mode==PMODE_INS_TGNSS) { + nx=ins->nx; + na=ins->nb; + } /* single to double-difference transformation matrix (D') */ D=zeros(nx,nx); - if ((nb=ddmat(rtk,D,ddsat,vflg,nv,0))<=0) { + if ((nb=ddmat(rtk,D,ddsat,vflg,nv,fixnolock))<=0) { - /* retry */ - if ((nb=ddmat(rtk,D,ddsat,vflg,nv,1))<=0) { - errmsg(rtk,"no valid double-difference\n"); - free(D); - return 0; - } + trace(2,"no valid double-difference\n"); + free(D); + return 0; } ny=na+nb; y=mat(ny,1); Qy=mat(ny,ny); DP=mat(ny,nx); b=mat(nb,2); db=mat(nb,1); Qb=mat(nb,nb); Qab=mat(na,nb); QQ=mat(na,nb); DD=zeros(ny,ny); index=imat(nb,2); + if (nny) *nny=ny; + /* transform single to double-differenced phase-bias (y=D'*x, Qy=D'*P*D) */ matmul("TN",ny, 1,nx,1.0,D ,x,0.0,y ); matmul("TN",ny,nx,nx,1.0,D ,P,0.0,DP); @@ -2350,14 +2437,15 @@ static int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa, ddsat_t *ddsat,in if ((nw=ddmat_WL(na,nb,ddsat,DD,index,wlsat))) { /* fix WL ambiguity */ - resamb_WL(rtk,Qy,y,ny,index,DD,nw,wlsat); + resamb_WL(rtk,Qy,y,ny,index,DD,nw,ddsat,wlsat,dxwl,Qywl); } } /* phase-bias covariance (Qb) and real-parameters to bias covariance (Qab) */ for (i=0;iopt; insopt_t *insopt=&rtk->opt.insopt; @@ -2557,11 +2662,12 @@ static int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,const nav_t *nav double *Ri,*Rj,dr[3]={0}; double *rs,*dts,*var,*y,*e,*azel; double *v,*H,*R,*xp,*Pp,*xa,*bias,dt,*x,*P,rr[3],*Pa,*dx; + double *dxwl,*Qywl; int i,j,k,f,n=nu+nr,ns,ny,nv=0,sat[MAXSAT],iu[MAXSAT],ir[MAXSAT],niter,nx,na; int info,vflg[MAXOBS*NFREQ*2+1],svh[MAXOBS*2]; int stat=rtk->opt.mode<=PMODE_DGPS?SOLQ_DGPS:SOLQ_FLOAT; int nf=opt->ionoopt==IONOOPT_IFLC?1:opt->nf,tc; - int ix,iy,iz,ivx,ivy,ivz,namb=0; + int ix,iy,iz,ivx,ivy,ivz,namb=0,nny=0; int nb[NFREQ*4*2+2]={0},b=0,m; int iba,ibg,nba,nbg; @@ -2586,6 +2692,7 @@ static int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,const nav_t *nav var=mat(1,n); y=mat(nf*2,n); e=mat(3,n); azel=zeros(2,n); Ri=mat(n*nf*2+2,1); Rj=mat(n*nf*2+2,1); + dxwl=mat(nx,1); Qywl=mat(nx,nx); for (i=0;issat[i].sys=(unsigned char)satsys(i+1,NULL); @@ -2651,7 +2758,8 @@ static int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,const nav_t *nav } /* double-difference residuals and partial derivatives */ if ((nv=ddres(rtk,nav,obs,dt,xp,Pp,sat,y,e,azel,iu,ir,ns,v,H,R,vflg,rr, - NULL,NULL,NULL,NULL,refsat))<1) { + NULL,NULL,NULL,NULL, + refsat,usedualfrq))<1) { errmsg(rtk,"no double-difference residual\n"); stat=SOLQ_NONE; break; @@ -2685,7 +2793,8 @@ static int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,const nav_t *nav /* post-fit residuals for float solution */ nv=ddres(rtk,nav,obs,dt,xp,Pp,sat,y,e,azel,iu,ir,ns,v,NULL, - R,vflg,rr,Ri,Rj,nb,&b,refsat); + R,vflg,rr,Ri,Rj,nb,&b, + refsat,usedualfrq); /* validation of float solution */ if (nv&&valpos(rtk,v,R,vflg,nv,&m,4.0)&&(tc?valins(opt,dx,ins):true)) { @@ -2694,6 +2803,8 @@ static int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,const nav_t *nav matcpy(P,Pp,nx,nx); matcpy(x,xp,nx, 1); + matcpy(dxwl,xp,1,nx); + /* update ambiguity control struct */ rtk->sol.ns=0; for (i=0;iopt.modear==ARMODE_TCARC) { if (resamb_TCAR(rtk,obs,sat,iu,ir,ns,nav,azel)) { stat=SOLQ_FIX; } } - /* resolve integer ambiguity by LAMBDA */ - else if (stat!=SOLQ_NONE&&resamb_LAMBDA(rtk,bias,xa,ddsat,&namb,vflg,nv)>1) { + /* resolve integer ambiguity by LAMBDA */ + else if (stat!=SOLQ_NONE&&resamb_LAMBDA2(rtk,bias,xa,ddsat,&namb,dxwl,Qywl,vflg,nv,&nny)>1) { if (tc) { /* adjust corrections */ - if (namb<4) { - for (j=iba;jsol.stat=(unsigned char)stat; + } + } + } + if ((rtk->sol.stat==SOLQ_WLFIX||rtk->sol.stat==SOLQ_INHERIT_WL)&&nny&&stat!=SOLQ_FIX) { + + /* WL fix to update rover position */ + if (tc) { + /* close loop for ins states */ + clp(&insp,insopt,dxwl); + + /* convert to gps antenna position */ + insp2antp(&insp,rr); + } + else { + matcpy(rr,dxwl,1,3); + } + /* check solutions */ + if (zdres(0,obs,nu,rs,dts,svh,nav,rr,opt,0,y,e,azel)) { + + /* post-fit residuals for fixed solution */ + nv=ddres(rtk,nav,obs,dt,dxwl,NULL,sat,y,e,azel,iu,ir,ns,v,NULL,R,vflg,rr, + NULL,NULL, + NULL,NULL, + refsat,usedualfrq); + + trace(3,"Qywl=\n"); + tracemat(3,Qywl,nny,nny,12,5); + + /* validation of fixed solution */ + if (nv&&valpos(rtk,v,R,vflg,nv,&m,4.0)&&(tc?valins(opt,xa,ins):true)) { + + if (!tc) { + matcpy(rtk->xa,dxwl,1,na); + for (i=0;iPa[i+j*na]=Qywl[i+j*nny]; + } + } + /* set fix flag */ + stat=SOLQ_FIX; + rtk->sol.ratio=rtk->sol.wlratio; } } } @@ -2929,7 +3081,6 @@ static int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,const nav_t *nav if (rtk->ssat[i].slip[j]&1) rtk->ssat[i].slipc[j]++; } if (stat!=SOLQ_NONE) { - rtk->sol.stat =(unsigned char)stat; rtk->ins.ns =(unsigned char)rtk->sol.ns; rtk->ins.gstat=(unsigned char)stat; } @@ -2938,6 +3089,7 @@ static int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,const nav_t *nav free(y ); free(e ); free(azel); free(v ); free(H ); free(bias); free(R ); free(Ri ); free(Rj); + free(dxwl); free(Qywl); return stat!=SOLQ_NONE; } /* initialize rtk control ---------------------------------------------------- @@ -3110,8 +3262,7 @@ extern int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) char msg[128]=""; trace(3,"rtkpos : time=%s n=%d\n",time_str(obs[0].time,3),n); - trace(4,"obs=\n"); traceobs(4,obs,n); - trace(5,"nav=\n"); tracenav(5,nav); + trace(3,"obs=\n"); traceobs(3,obs,n); double sow; sow=time2gpst(obs[0].time,NULL); @@ -3155,8 +3306,8 @@ extern int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) fk=2; } if (opt->adjobs) { - trace(4,"adjust obs=\n"); - traceobs(4,obsd,n); + trace(3,"adjust obs=\n"); + traceobs(3,obsd,n); } /* previous epoch */ time=rtk->sol.time; @@ -3236,13 +3387,15 @@ extern int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) } } /* relative positioning */ - stat=relpos(rtk,opt->adjobs?obsd:obs,nu,nr,nav,0); - + stat=relpos(rtk,opt->adjobs?obsd:obs,nu,nr,nav,0,1); + if (!stat) { + stat=relpos(rtk,opt->adjobs?obsd:obs,nu,nr,nav,0,0); + } #if 1 /* degrade to dgps-tc mode if rtk-tc fail */ if (stat==0&&opt->mode==PMODE_INS_TGNSS) { insopt->tc=INSTC_DGPS; - stat=relpos(rtk,opt->adjobs?obsd:obs,nu,nr,nav,1); + stat=relpos(rtk,opt->adjobs?obsd:obs,nu,nr,nav,1,1); insopt->tc=INSTC_RTK; if (stat) goto exit; } diff --git a/src/ins-gnss/tcpostpos.cc b/src/ins-gnss/tcpostpos.cc index d548492..c2e11ad 100644 --- a/src/ins-gnss/tcpostpos.cc +++ b/src/ins-gnss/tcpostpos.cc @@ -22,7 +22,7 @@ static int nimu=0; /* number of imu measurements epochs */ static int iobsu =0; /* current rover observation data index */ static int iobsr =0; /* current reference observation data index */ static int iimu =0; /* current imu measurement data */ -static int keepalive=0; /* keep alive flag */ +static int keepalive=1; /* keep alive flag */ static int timeout =10000; /* timeout time (ms) */ static int reconnect=10000; /* reconnect interval (ms) */ static int week=0; /* GPS week */