diff --git a/src/config_f9p.py b/src/config_f9p.py index 833d68e..b219c16 100644 --- a/src/config_f9p.py +++ b/src/config_f9p.py @@ -22,7 +22,7 @@ # ------------ Kalman Filter Statistics ------------------------ eratio = [300, 300] # ratio between psuedorange noise and carrier phase noise for L1, L2 -efact = {uGNSS.GPS: 1.0, uGNSS.GLO: 1.5, uGNSS.GAL: 1.0} # relative weighting of each constellation +efact = {uGNSS.GPS: 1.0, uGNSS.GLO: 1.5, uGNSS.GAL: 1.0, uGNSS.BDS: 1.0} # relative weighting of each constellation err = [0, 0.003, 0.003, 0.0, 0, 0, 5e-12] # error sigmas [-, base, el, bl, snr, rcvstd, satclk] snrmax = 52 # max signal strength for variance calc (dB-Hz) accelh = 3 # horiz accel noise sigma (m/sec2) @@ -65,31 +65,34 @@ # ----------- Configure observation signals ---------------- - -gnss_t = [uGNSS.GPS, uGNSS.GLO, uGNSS.GAL] +gnss_t = [uGNSS.GPS, uGNSS.GAL, uGNSS.GLO] # Valid signals sig_tbl = {'1C': rSIG.L1C, '1X': rSIG.L1X, '1W': rSIG.L1W, '2W': rSIG.L2W, '2C': rSIG.L2C, '2X': rSIG.L2X, '5Q': rSIG.L5Q, '5X': rSIG.L5X, '7Q': rSIG.L7Q, - '7X': rSIG.L7X} + '7X': rSIG.L7X, '2I': rSIG.L2I, '7I': rSIG.L7I} skip_sig_tbl = {uGNSS.GPS: [], # skip these obs uGNSS.GLO: [], uGNSS.GAL: [], + uGNSS.BDS: [], uGNSS.QZS: []} # set these from table below -freq_ix0 = {uGNSS.GPS: 0, uGNSS.GLO: 4, uGNSS.GAL: 0} # L1 -freq_ix1 = {uGNSS.GPS: 1, uGNSS.GLO: 5, uGNSS.GAL: 3} # L2/E5b - +freq_ix0 = {uGNSS.GPS: 0, uGNSS.GLO: 4, uGNSS.GAL: 0, uGNSS.BDS: 6} # L1 +freq_ix1 = {uGNSS.GPS: 1, uGNSS.GLO: 5, uGNSS.GAL: 3, uGNSS.BDS: 7} # L2/E5b # ---------- Frequencies currently supported------------- freq = [1.57542e9, # L1/E1 1.22760e9, # L2 1.17645e9, # L5/E5a/B2a 1.20714e9, # E5b 1.60200E9, # G1 - 1.24600E9] # G2 + 1.24600E9, # G2 + 1.561098E9, # B1I + 1.20714E9, # B2I/B2b + ] + dfreq_glo = [0.56250E6, 0.43750E6] # L1, L2 diff --git a/src/ephemeris.py b/src/ephemeris.py index c68ae77..cf0d687 100644 --- a/src/ephemeris.py +++ b/src/ephemeris.py @@ -17,13 +17,14 @@ def seleph(nav, t, sat): - """ select ephemeric for sat, assumes ephemeris is sorted by sat, then time """ + """ select ephemeric for sat, assume ephemeris is sorted by sat, then time """ dt_p = 1e10 # timediff(t, nav.eph[nav.eph_index[sat]].toe) eph = None - sys = sat2prn(sat)[0] - i_p = 0 + sys = sat2prn(sat)[0] + i_p = 0 if sys != uGNSS.GLO: # start with previous index for this sat + for i, eph_ in enumerate(nav.eph[nav.eph_index[sat]:]): if eph_.sat != sat: continue @@ -80,23 +81,28 @@ def sva2ura(sys, sva): def eph2pos(t, eph): """ broadcast ephemeris to satellite position and clock bias ------------- -* compute satellite position and clock bias with broadcast ephemeris (gps, -* galileo, qzss) -* args : gtime_t time I time (gpst) -* eph_t *eph I broadcast ephemeris -* double *rs O satellite position (ecef) {x,y,z} (m) -* double *dts O satellite clock bias (s) -* double *var O satellite position and clock variance (m^2) -* return : none -* notes : see ref [1],[7],[8] -* satellite clock includes relativity correction without code bias -* (tgd or bgd) """ + * compute satellite position and clock bias with broadcast ephemeris (gps, + * galileo, qzss, bds) + * args : gtime_t time I time (gpst) + * eph_t *eph I broadcast ephemeris + * double *rs O satellite position (ecef) {x,y,z} (m) + * double *dts O satellite clock bias (s) + * double *var O satellite position and clock variance (m^2) + * return : none + * notes : see ref [1],[7],[8],[9] + * satellite clock includes relativity correction without code bias + * (tgd or bgd) + """ tk = dtadjust(t, eph.toe) - sys, _ = sat2prn(eph.sat) + sys, prn = sat2prn(eph.sat) + if sys == uGNSS.GAL: mu = rCST.MU_GAL omge = rCST.OMGE_GAL - else: # GPS,QZS + elif sys == uGNSS.BDS: + mu = rCST.MU_BDS + omge = rCST.OMGE_BDS + else: # GPS, QZSS mu = rCST.MU_GPS omge = rCST.OMGE @@ -122,16 +128,31 @@ def eph2pos(t, eph): x = r * np.cos(u) y = r * np.sin(u) cosi = np.cos(i) - O = eph.OMG0 + (eph.OMGd - omge) * tk - omge * eph.toes - sinO, cosO = np.sin(O), np.cos(O) - rs = [x * cosO - y * cosi * sinO, x * sinO + y * cosi * cosO, y * np.sin(i)] + + if sys == uGNSS.BDS and (prn <= 145 or prn >= 199): + O = eph.OMG0 + eph.OMGd * tk - omge * eph.toes + sinO, cosO = np.sin(O), np.cos(O) + sin5, cos5 = np.sin(5), np.cos(5) + xg = x * cosO - y * cosi * sinO + yg = x * sinO + y * cosi * cosO + zg = y * np.sin(i) + sino = np.sin(omge * tk) + coso = np.cos(omge * tk) + rs = [xg * coso + yg * sino * cos5 + zg * sino * sin5, + -xg * sino + yg * coso * cos5 + zg * coso * sin5, + -yg * sin5 + zg * cos5] + else: + O = eph.OMG0 + (eph.OMGd - omge) * tk - omge * eph.toes + sinO, cosO = np.sin(O), np.cos(O) + rs = [x * cosO - y * cosi * sinO, x * sinO + y * cosi * cosO, y * np.sin(i)] + tk = dtadjust(t, eph.toc) dts = eph.f0 + eph.f1 * tk + eph.f2 * tk**2 # relativity correction - dts -= 2 *np.sqrt(mu * eph.A) * eph.e * sinE / rCST.CLIGHT**2 + dts -= 2 * np.sqrt(mu * eph.A) * eph.e * sinE / rCST.CLIGHT**2 var = sva2ura(sys, eph.sva) trace(4, 'eph2pos: sat=%d, dts=%.10f rs=%.4f %.4f %.4f var=%.3f\n' % - (eph.sat, dts,rs[0],rs[1],rs[2], var)) + (eph.sat, dts, rs[0], rs[1], rs[2], var)) return rs, var, dts def deq(x, acc): diff --git a/src/pntpos.py b/src/pntpos.py index 994894d..c6b2d0d 100644 --- a/src/pntpos.py +++ b/src/pntpos.py @@ -12,7 +12,7 @@ from ephemeris import seleph, satposs from rinex import rcvstds -NX = 6 # num of estimated parameters, pos + clock +NX = 4+4 # num of estimated parameters, pos + clock MAXITR = 10 # max number of iteration or point pos ERR_ION = 5.0 # ionospheric delay Std (m) ERR_TROP = 3.0 # tropspheric delay Std (m) @@ -122,6 +122,10 @@ def rescode(iter, obs, nav, rs, dts, svh, x): v[nv] -= x[5] H[nv, 5] = 1.0 mask[2] = 1 + elif sys == uGNSS.BDS: + v[nv] -= x[6] + H[nv, 6] = 1.0 + mask[3] = 1 else: mask[0] = 1 diff --git a/src/rinex.py b/src/rinex.py index f93ad06..09a7790 100644 --- a/src/rinex.py +++ b/src/rinex.py @@ -8,7 +8,7 @@ import numpy as np from copy import deepcopy from rtkcmn import uGNSS, rSIG, Eph, Geph, prn2sat, gpst2time, time2gpst, Obs, \ - epoch2time, timediff, timeadd, utc2gpst + epoch2time, timediff, timeadd, utc2gpst, bdt2gpst import rtkcmn as gn from ephemeris import satposs @@ -19,7 +19,8 @@ class rnx_decode: def __init__(self, cfg): self.ver = -1.0 self.fobs = None - self.gnss_tbl = {'G': uGNSS.GPS, 'E': uGNSS.GAL, 'R': uGNSS.GLO, 'J': uGNSS.QZS} + #self.gnss_tbl = {'G': uGNSS.GPS, 'E': uGNSS.GAL, 'R': uGNSS.GLO, 'C': uGNSS.BDS, 'J': uGNSS.QZS} + self.gnss_tbl = {'G': uGNSS.GPS, 'E': uGNSS.GAL, 'R': uGNSS.GLO, 'C': uGNSS.BDS, 'J': uGNSS.QZS} self.sig_tbl = cfg.sig_tbl self.skip_sig_tbl = cfg.skip_sig_tbl self.nf = 4 @@ -37,7 +38,6 @@ def flt(self, u, c=-1): except: return 0 - def adjday(self, t, t0): """" adjust time considering week handover """ tt = timediff(t, t0) @@ -74,6 +74,8 @@ def decode_nav(self, navfile, nav): prn = int(line[1:3]) if sys == uGNSS.QZS: prn += 192 + elif sys == uGNSS.BDS: + prn += 140 sat = prn2sat(sys, prn) year = int(line[4:8]) month = int(line[9:11]) @@ -124,7 +126,7 @@ def decode_nav(self, navfile, nav): eph.svh = int(self.flt(line, 1)) tgd = np.zeros(2) tgd[0] = float(self.flt(line, 2)) - if sys == uGNSS.GAL: + if sys == uGNSS.GAL or sys == uGNSS.BDS: tgd[1] = float(self.flt(line, 3)) else: eph.iodc = int(self.flt(line, 3)) @@ -134,9 +136,14 @@ def decode_nav(self, navfile, nav): tot = int(self.flt(line, 0)) if len(line) >= 42: eph.fit = int(self.flt(line, 1)) - - eph.toe = gpst2time(eph.week, eph.toes) - eph.tot = gpst2time(eph.week, tot) + if sys == uGNSS.BDS: + eph.toc = bdt2gpst(toc) + eph.toe = bdt2gpst(gpst2time(eph.week, eph.toes)) + eph.tot = bdt2gpst(gpst2time(eph.week, tot)) + eph.iodc = int(self.flt(line, 1)) + else: + eph.toe = gpst2time(eph.week, eph.toes) + eph.tot = gpst2time(eph.week, tot) nav.eph.append(eph) else: # GLONASS if prn > uGNSS.GLOMAX: @@ -185,8 +192,6 @@ def decode_nav(self, navfile, nav): geph.acc = acc * 1000 nav.geph.append(geph) - - #nav.eph.sort(key=lambda x: (x.sat, x.toe.time)) nav.eph.sort(key=lambda x: x.toe.time) nav.geph.sort(key=lambda x: x.toe.time) return nav @@ -274,7 +279,10 @@ def decode_obs(self, nav, maxepoch): prn = int(line[1:3]) if sys == uGNSS.QZS: prn += 192 - obs.sat[n] = prn2sat(sys, prn) + elif sys == uGNSS.BDS: + prn += 140 + sat = prn2sat(sys, prn) + obs.sat[n] = sat if obs.sat[n] == 0: continue nsig_max = (len(line) - 4 + 2) // 16 @@ -383,4 +391,3 @@ def rcvstds(nav, obs): nav.rcvstd[s,f] = obs.Lstd[i,f] * 0.004 * 0.2 # Pstd: 0.01*2^(n+5) nav.rcvstd[s,f+nav.nf] = 0.01 * (1 << (obs.Pstd[i,f] + 5)) - diff --git a/src/rtkcmn.py b/src/rtkcmn.py index f804047..83c5f37 100644 --- a/src/rtkcmn.py +++ b/src/rtkcmn.py @@ -46,12 +46,14 @@ class rCST(): MU_GPS = 3.9860050E14 MU_GAL = 3.986004418E14 MU_GLO = 3.9860044E14 + MU_BDS = 3.986004418E14 GME = 3.986004415E+14 GMS = 1.327124E+20 GMM = 4.902801E+12 OMGE = 7.2921151467E-5 OMGE_GAL = 7.2921151467E-5 OMGE_GLO = 7.292115E-5 + OMGE_BDS = 7.292115E-5 RE_WGS84 = 6378137.0 RE_GLO = 6378136.0 FE_WGS84 = (1.0/298.257223563) @@ -77,10 +79,10 @@ class uGNSS(IntEnum): GALMAX = 36 QZSMAX = 10 GLOMAX = 27 -# BDSMAX = 63 -# SBSMAX = 24 -# IRNMAX = 10 - BDSMAX = 0 + BDSMAX = 63 + #SBSMAX = 24 + #IRNMAX = 10 + #BDSMAX = 0 SBSMAX = 0 IRNMAX = 0 NONE = -1 @@ -120,14 +122,16 @@ class rSIG(IntEnum): L1C = 1 L1X = 2 L1W = 3 - L2C = 4 - L2L = 5 - L2X = 6 - L2W = 7 - L5Q = 8 - L5X = 9 - L7Q = 10 - L7X = 11 + L2I = 4 + L2C = 5 + L2L = 6 + L2X = 7 + L2W = 8 + L5Q = 9 + L5X = 10 + L7Q = 11 + L7X = 12 + L7I = 13 SIGMAX = 16 @@ -164,6 +168,7 @@ class Eph(): f2 = 0.0 toc = 0 toe = 0 + ttr = 0 tot = 0 week = 0 crs = 0.0 @@ -362,6 +367,10 @@ def timediff(t1: gtime_t, t2: gtime_t): return dt +def bdt2gpst(t: gtime_t) -> gtime_t: + """Convert BeiDou time (BDT) to GPS time (GPST)""" + return timeadd(t, 14.0) + def gpst2time(week, tow): """ convert to time from gps-time """ t = epoch2time(gpst0) @@ -422,7 +431,7 @@ def prn2sat(sys, prn): elif sys == uGNSS.GAL: sat = prn+uGNSS.GPSMAX+uGNSS.GLOMAX elif sys == uGNSS.BDS: - sat = prn+uGNSS.GPSMAX+uGNSS.GLOMAX+uGNSS.GALMAX + sat = prn-140+uGNSS.GPSMAX+uGNSS.GLOMAX+uGNSS.GALMAX elif sys == uGNSS.QZS: sat = prn-192+uGNSS.GPSMAX+uGNSS.GLOMAX+uGNSS.GALMAX+uGNSS.BDSMAX else: @@ -436,7 +445,7 @@ def sat2prn(sat): prn = sat-(uGNSS.GPSMAX+uGNSS.GLOMAX+uGNSS.GALMAX+uGNSS.BDSMAX)+192 sys = uGNSS.QZS elif sat > uGNSS.GPSMAX+uGNSS.GLOMAX+uGNSS.GALMAX: - prn = sat-(uGNSS.GPSMAX+uGNSS.GLOMAX+uGNSS.GALMAX) + prn = sat-(uGNSS.GPSMAX+uGNSS.GLOMAX+uGNSS.GALMAX)+140 sys = uGNSS.BDS elif sat > uGNSS.GPSMAX+uGNSS.GLOMAX: prn = sat-(uGNSS.GPSMAX+uGNSS.GLOMAX) @@ -457,6 +466,8 @@ def sat2id(sat): uGNSS.QZS: 'J', uGNSS.GLO: 'R'} if sys == uGNSS.QZS: prn -= 192 + elif sys == uGNSS.BDS: + prn -= 140 elif sys == uGNSS.SBS: prn -= 100 return '%s%02d' % (gnss_tbl[sys], prn) @@ -464,18 +475,18 @@ def sat2id(sat): def id2sat(id_): """ convert id to satellite number """ - # gnss_tbl={'G':uGNSS.GPS,'S':uGNSS.SBS,'E':uGNSS.GAL,'C':uGNSS.BDS, - # 'I':uGNSS.IRN,'J':uGNSS.QZS,'R':uGNSS.GLO} - gnss_tbl = {'G': uGNSS.GPS, 'E': uGNSS.GAL, 'C': uGNSS.BDS, - 'J': uGNSS.QZS, 'R': uGNSS.GLO} + gnss_tbl={'G':uGNSS.GPS,'S':uGNSS.SBS,'E':uGNSS.GAL,'C':uGNSS.BDS, + 'I':uGNSS.IRN,'J':uGNSS.QZS,'R':uGNSS.GLO} + #gnss_tbl = {'G': uGNSS.GPS, 'E': uGNSS.GAL, 'C': uGNSS.BDS, + # 'J': uGNSS.QZS, 'R': uGNSS.GLO} if id_[0] not in gnss_tbl: return -1 sys = gnss_tbl[id_[0]] prn = int(id_[1:3]) if sys == uGNSS.QZS: prn += 192 - elif sys == uGNSS.SBS: - prn += 100 + elif sys == uGNSS.BDS: + prn += 140 sat = prn2sat(sys, prn) return sat diff --git a/src/rtkpos.py b/src/rtkpos.py index 0b7fbf8..8d4b1df 100644 --- a/src/rtkpos.py +++ b/src/rtkpos.py @@ -1010,7 +1010,7 @@ def relpos(nav, obsr, obsb, sol): nav.ns = len(ix) # valid satellite count by L1 # check for too few valid phases if nav.ns < 4: - stat = gn.SOLQ_DGPS; + stat = gn.SOLQ_DGPS # resolve integer ambiguity by LAMBDA if nav.armode > 0 and stat == gn.SOLQ_FLOAT: