Skip to content

Commit

Permalink
改善模糊度固定策略,效果较好
Browse files Browse the repository at this point in the history
  • Loading branch information
@Erensu
Erensu committed Jun 2, 2019
1 parent 6508c03 commit 7f4237c
Show file tree
Hide file tree
Showing 12 changed files with 380 additions and 153 deletions.
22 changes: 13 additions & 9 deletions include/carvig.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
Expand Down Expand Up @@ -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 */

Expand Down Expand Up @@ -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 */
Expand Down Expand Up @@ -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 */
Expand All @@ -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 {
Expand All @@ -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;
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down
2 changes: 1 addition & 1 deletion plot/plot.ini
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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/::
Expand Down
22 changes: 11 additions & 11 deletions src/ins-gnss/app/carvig-rtk.conf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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 =

Expand All @@ -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)
Expand Down Expand Up @@ -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_???)
Expand Down Expand Up @@ -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
Expand Down
12 changes: 6 additions & 6 deletions src/ins-gnss/app/carvig-tc.conf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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 =

Expand All @@ -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 =
Expand Down Expand Up @@ -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
Expand Down
5 changes: 3 additions & 2 deletions src/ins-gnss/app/rnx2rtkp.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;i<argc;i++) {
if (!strcmp(argv[i],"-p")&&i+1<argc) strcpy(file,argv[++i]);
else if (!strcmp(argv[i],"-o")&&i+1<argc) outfile=argv[++i];
else if (!strcmp(argv[i],"-m")&&i+1<argc) port=atoi(argv[++i]);
else printhelp();
}
/* load options file */
Expand Down Expand Up @@ -144,7 +145,7 @@ int main(int argc, char **argv)
showmsg("error : no input file");
return -2;
}
ret=postpos(ts,te,tint,0.0,&prcopt,&solopt,&filopt,infile,n,outfile,"","");
ret=postpos(ts,te,tint,0.0,&prcopt,&solopt,&filopt,infile,n,outfile,"","",port);

if (!ret) fprintf(stderr,"%40s\r","");
for (i=0;i<MAXFILE;i++) {
Expand Down
7 changes: 4 additions & 3 deletions src/ins-gnss/ins-gnss-tc.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -155,7 +155,7 @@ extern int tcigpos(const prcopt_t *opt,const obsd_t *obs,int n,const nav_t *nav,
const imud_t *imu,rtk_t *rtk,insstate_t *ins,int upd)
{
int i,nx=ins->nx,info=1,nu,nr,flag;
double *P,dt;
double dt;
const insopt_t* insopt=&opt->insopt;

if (imu==NULL) return 0;
Expand Down Expand Up @@ -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 */
Expand All @@ -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
Expand Down Expand Up @@ -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;
}

8 changes: 3 additions & 5 deletions src/ins-gnss/pntpos.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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';

Expand Down
Loading

0 comments on commit 7f4237c

Please sign in to comment.