From 2e4cda3e2bc1652c7ebc499b432a7cb83a1799da Mon Sep 17 00:00:00 2001 From: Rui Hirokawa Date: Sun, 19 Dec 2021 16:58:39 +0900 Subject: [PATCH] - added monitor - kinematic case test for ppp-rtk. --- cssrlib/cssrlib.py | 6 +- cssrlib/gnss.py | 2 +- cssrlib/ppprtk.py | 111 +++++++++++++++++++------------- cssrlib/rtk.py | 4 +- cssrlib/tests/test_ppprtk.py | 9 ++- cssrlib/tests/test_ppprtk2.py | 115 ++++++++++++++++++++++++++++++++++ cssrlib/tests/test_rtk.py | 4 +- cssrlib/tests/test_rtk2.py | 2 +- setup.py | 10 +-- 9 files changed, 202 insertions(+), 61 deletions(-) create mode 100644 cssrlib/tests/test_ppprtk2.py diff --git a/cssrlib/cssrlib.py b/cssrlib/cssrlib.py index 7836347..e0b4bf3 100644 --- a/cssrlib/cssrlib.py +++ b/cssrlib/cssrlib.py @@ -546,7 +546,7 @@ def decode_cssr_stec(self, msg, i): self.flg_net = True dfm = bs.unpack_from_dict('u2u5u'+str(self.nsat_n), ['stype', 'inet', 'svmaskn'], msg, i) - self.inet = inet = dfm['inet'] + inet = dfm['inet'] self.netmask[inet] = netmask = dfm['svmaskn'] self.lc[inet].sat_n = self.decode_local_sat(netmask) self.lc[inet].nsat_n = nsat = len(self.lc[inet].sat_n) @@ -573,7 +573,7 @@ def decode_cssr_grid(self, msg, i): ['ttype', 'range', 'inet', 'svmaskn', 'class', 'value', 'ng'], msg, i) self.flg_net = True - self.inet = inet = dfm['inet'] + inet = dfm['inet'] self.netmask[inet] = netmask = dfm['svmaskn'] self.lc[inet].sat_n = self.decode_local_sat(netmask) self.lc[inet].nsat_n = nsat = len(self.lc[inet].sat_n) @@ -668,7 +668,7 @@ def decode_cssr_atmos(self, msg, i): dfm = bs.unpack_from_dict('u2u2u5u6', ['trop', 'stec', 'inet', 'ng'], msg, i) self.flg_net = True - self.inet = inet = dfm['inet'] + inet = dfm['inet'] self.lc[inet].ng = ng = dfm['ng'] self.lc[inet].flg_trop = dfm['trop'] self.lc[inet].flg_stec = dfm['stec'] diff --git a/cssrlib/gnss.py b/cssrlib/gnss.py index 331faee..723224f 100644 --- a/cssrlib/gnss.py +++ b/cssrlib/gnss.py @@ -191,7 +191,7 @@ def __init__(self): -0.46, 1.58, 4.16, 7.70, 12.53], [+0.00, -0.16, -0.60, -1.26, -2.06, -2.91, -3.77, -4.57, -5.21, -5.54, -5.43, -4.81, -3.69, -2.21, - -0.46, +1.58, +4.16, +7.70, +12.53]] + -0.46, 1.58, 4.16, 7.70, 12.53]] self.ant_pco = [+85.44, +115.05, +115.05] # antenna type: TRM59800.80 NONE [mm] 0:5:90 [deg] diff --git a/cssrlib/ppprtk.py b/cssrlib/ppprtk.py index 7e43a73..2a1b4b6 100644 --- a/cssrlib/ppprtk.py +++ b/cssrlib/ppprtk.py @@ -11,7 +11,7 @@ from cssrlib.ephemeris import satposs from cssrlib.cssrlib import sSigGPS, sSigGAL, sSigQZS, sCType from cssrlib.ppp import tidedisp, shapiro, windupcorr -from cssrlib.rtk import IB, ddres, resamb_lambda, valpos, holdamb, initx, kfupdate +from cssrlib.rtk import IB, ddres, resamb_lambda, valpos, holdamb, initx MAXITR = 10 ELMIN = 10 @@ -34,14 +34,26 @@ def logmon(nav, t, sat, cs, iu=None): for i in range(n[0]): if cpc[i, 0] == 0 and cpc[i, 1] == 0: continue - nav.fout.write("%6d\t%3d\t%8.3f\t%8.3f\t%8.3f\t%8.3f\t%2d\t" - % (tow, sat[i], cpc[i, 0], cpc[i, 1], - prc[i, 0], prc[i, 1], cs.iodssr)) + sys, prn = gn.sat2prn(sat[i]) + pb = osr[i, 0:2] + cb = osr[i, 2:4] + antr = osr[i, 4:6] + phw = osr[i, 6:8] + trop = osr[i, 8] + iono = osr[i, 9] + relatv = osr[i, 10] + dorb = osr[i, 11] + dclk = osr[i,12] + # tow sys prn trop iono antr1 antr2 antr5 relatv + # wup2 wup5 CPC1 CPC2 CPC5 PRC1 PRC2 PRC5 orb clk + nav.fout.write("%6d\t%2d\t%3d\t%8.3f\t%8.3f\t%8.3f\t%8.3f\t" + % (tow, sys, prn, pb[0], pb[1], cb[0], cb[1])) + nav.fout.write("%8.3f\t%8.3f\t%8.3f\t%8.3f\t" + % (trop, iono, antr[0], antr[1])) nav.fout.write("%8.3f\t%8.3f\t%8.3f\t%8.3f\t%8.3f\t" - % (osr[i, 0], osr[i, 1], osr[i, 2], - osr[i, 3], osr[i, 4])) + % (relatv, phw[0], phw[1], cpc[i, 0], cpc[i, 1])) nav.fout.write("%8.3f\t%8.3f\t%8.3f\t%8.3f\n" - % (osr[i, 5], osr[i, 6], osr[i, 7], osr[i, 8])) + % (prc[i, 0], prc[i, 1], dorb, dclk)) return 0 @@ -51,7 +63,7 @@ def rtkinit(nav, pos0=np.zeros(3)): nav.pmode = 1 # 0:static, 1:kinematic nav.na = 3 if nav.pmode == 0 else 6 - nav.nq = 3 + nav.nq = 3 if nav.pmode == 0 else 6 nav.ratio = 0 nav.thresar = [2] nav.nx = nav.na+gn.uGNSS.MAXSAT*nav.nf @@ -63,8 +75,8 @@ def rtkinit(nav, pos0=np.zeros(3)): nav.phw = np.zeros(gn.uGNSS.MAXSAT) # parameter for PPP-RTK - nav.eratio = [50, 50] - nav.err = [100, 0.00707, 0.00354] + nav.eratio = [100, 100] + nav.err = [0, 0.003, 0.003] nav.sig_p0 = 30.0 nav.sig_v0 = 10.0 nav.sig_n0 = 30.0 @@ -73,18 +85,20 @@ def rtkinit(nav, pos0=np.zeros(3)): nav.tidecorr = True nav.armode = 1 # 1:contunous,2:instantaneous,3:fix-and-hold nav.gnss_t = [uGNSS.GPS, uGNSS.GAL, uGNSS.QZS] - nav.gnss_t = [uGNSS.GPS] # GPS only + # nav.gnss_t = [uGNSS.GPS] # GPS only # nav.x[0:3] = pos0 + nav.x[3:6] = 0.0 dP = np.diag(nav.P) dP.flags['WRITEABLE']=True dP[0:3] = nav.sig_p0**2 nav.q = np.zeros(nav.nq) - if nav.pmode >= 1: + if nav.pmode >= 1: # kinematic dP[3:6] = nav.sig_v0**2 - nav.q[0:3] = nav.sig_qv**2 + nav.q[0:3] = nav.sig_qp**2 + nav.q[3:6] = nav.sig_qv**2 else: nav.q[0:3] = nav.sig_qp**2 # obs index @@ -119,20 +133,13 @@ def udstate(nav, obs, cs): # pos,vel na = nav.na - Px = nav.P[0:na, 0:na] - if nav.pmode >= 1: - F = np.eye(na) - F[0:3, 3:6] = np.eye(3)*tt - nav.x[0:3] += tt*nav.x[3:6] - Px = F@Px@F.T - dP = np.diag(Px) - dP.flags['WRITEABLE'] = True - dP[3:3+nav.nq] += nav.q[0:nav.nq]*tt - else: - dP = np.diag(Px) - dP.flags['WRITEABLE'] = True - dP[0:nav.nq] += nav.q[0:nav.nq]*tt - nav.P[0:na, 0:na] = Px + + Phi = np.eye(nav.nx) + Phi[0:3,3:6]=np.eye(3)*tt + nav.P = Phi@nav.P@Phi.T + dP = np.diag(nav.P) + dP.flags['WRITEABLE'] = True + dP[0:nav.nq] += nav.q[0:nav.nq]*tt # bias for f in range(nav.nf): @@ -141,13 +148,14 @@ def udstate(nav, obs, cs): for i in range(gn.uGNSS.MAXSAT): sat_ = i+1 nav.outc[i, f] += 1 - reset = True if nav.outc[i, f] > nav.maxout else False + reset = (nav.outc[i, f] > nav.maxout) sys_i, _ = gn.sat2prn(sat_) if sys_i not in nav.gnss_t: continue j = IB(sat_, f, nav.na) if reset and nav.x[j] != 0.0: - initx(nav, 0.0, 0.0, j) + #initx(nav, 0.0, 0.0, j) + #print("reset amb f=%d sat=%d outc=%d" % (f,sat_,nav.outc[i, f])) nav.outc[i, f] = 0 # cycle slip check by LLI for i in range(ns): @@ -164,7 +172,7 @@ def udstate(nav, obs, cs): initx(nav, 0.0, 0.0, IB(sat[i], f, nav.na)) # bias bias = np.zeros(ns) - offset = 0 + offset = 0 na = 0 for i in range(ns): if sys[i] not in nav.gnss_t: @@ -223,7 +231,7 @@ def zdres(nav, obs, rs, vs, dts, svh, rr, cs): cs.cpc = np.zeros((n, nf)) cs.prc = np.zeros((n, nf)) - cs.osr = np.zeros((n, 2*nf+5)) + cs.osr = np.zeros((n, 4*nf+5)) for i in range(n): sat = obs.sat[i] @@ -281,10 +289,10 @@ def zdres(nav, obs, rs, vs, dts, svh, rr, cs): cbias += cs.lc[inet].cbias[idx_l][kidx] if cs.lc[inet].pbias is not None: pbias += cs.lc[inet].pbias[idx_l][kidx] - # t1 = timediff(obs.t, cs.lc[0].t0[sCType.ORBIT]) - # t2 = timediff(obs.t, cs.lc[inet].t0[sCType.PBIAS]) - # if t1 >= 0 and t1 < 30 and t2 >= 30: - # pbias += nav.dsis[sat] + #t1 = timediff(obs.t, cs.lc[0].t0[sCType.ORBIT]) + #t2 = timediff(obs.t, cs.lc[inet].t0[sCType.PBIAS]) + #if t1 >= 0 and t1 < 30 and t2 >= 30: + # pbias += nav.dsis[sat] # relativity effect relatv = shapiro(rs[i, :], rr_) @@ -302,8 +310,9 @@ def zdres(nav, obs, rs, vs, dts, svh, rr, cs): # prc_c += nav.dorb[sat]-nav.dclk[sat] cs.prc[i, :] = prc_c+iono+cbias cs.cpc[i, :] = prc_c-iono+pbias+phw - cs.osr[i, :] = [pbias[0], pbias[1], cbias[0], cbias[1], trop, - iono_, relatv, nav.dorb[sat], nav.dclk[sat]] + cs.osr[i, :] = [pbias[0], pbias[1], cbias[0], cbias[1], + antr[0], antr[1], phw[0], phw[1], + trop, iono_, relatv, nav.dorb[sat], nav.dclk[sat]] r += -_c*dts[i] for f in range(nf): @@ -314,12 +323,22 @@ def zdres(nav, obs, rs, vs, dts, svh, rr, cs): return y, e, el +def kfupdate(x, P, H, v, R): + """ kalmanf filter measurement update """ + PHt = P@H.T + S = H@PHt+R + K = PHt@np.linalg.inv(S) + x += K@v + P = P - K@H@P + + return x, P, S + def ppprtkpos(nav, obs, cs): """ PPP-RTK positioning """ - for i in range(gn.uGNSS.MAXSAT): - for j in range(nav.nf): - nav.vsat[j] = 0 +# for i in range(gn.uGNSS.MAXSAT): +# for j in range(nav.nf): +# nav.vsat[j] = 0 rs, vs, dts, svh = satposs(obs, nav, cs) # Kalman filter time propagation @@ -337,6 +356,7 @@ def ppprtkpos(nav, obs, cs): el = elu[iu] nav.sat = sat nav.y = y + ns = len(sat) logmon(nav, obs.t, sat, cs, iu) @@ -365,16 +385,19 @@ def ppprtkpos(nav, obs, cs): if valpos(nav, v, R): nav.x = xp nav.P = Pp - ns = len(nav.sat) nav.ns = 0 for i in range(ns): + j = sat[i]-1 for f in range(nav.nf): - if nav.vsat[sat[i]-1, f] == 0: + if nav.vsat[j,f] == 0: continue - nav.lock[sat[i]-1, f] += 1 - nav.outc[sat[i]-1, f] = 0 + nav.lock[j,f] += 1 + nav.outc[j,f] = 0 if f==0: nav.ns += 1 + + else: + nav.smode = 0 nb, xa = resamb_lambda(nav, sat) nav.smode = 5 # float diff --git a/cssrlib/rtk.py b/cssrlib/rtk.py index 42011c1..14afd91 100644 --- a/cssrlib/rtk.py +++ b/cssrlib/rtk.py @@ -35,7 +35,7 @@ def rtkinit(nav, pos0=np.zeros(3)): nav.sig_v0 = 10.0 nav.sig_n0 = 30.0 nav.sig_qp = 0.01 - nav.sig_qv = 0.5 + nav.sig_qv = 0.01 nav.armode = 1 # 1:contunous,2:instantaneous,3:fix-and-hold nav.x[0:3] = pos0 @@ -566,7 +566,7 @@ def relpos(nav, obs, obsb): nav.smode = 0 nb, xa = resamb_lambda(nav, sat) - nav.smode = 5 + nav.smode = 5 # float if nb > 0: yu, eu, _ = zdres(nav, obs, rs, dts, svh, xa[0:3]) y[:ns, :] = yu[iu, :] diff --git a/cssrlib/tests/test_ppprtk.py b/cssrlib/tests/test_ppprtk.py index 62eb448..1dfc614 100644 --- a/cssrlib/tests/test_ppprtk.py +++ b/cssrlib/tests/test_ppprtk.py @@ -25,7 +25,9 @@ nav = Nav() nav = dec.decode_nav(navfile, nav) # nep=3600//30 -nep = 300 +nep = 180 +#nep = 60 + t = np.zeros(nep) tc = np.zeros(nep) enu = np.ones((nep, 3))*np.nan @@ -42,6 +44,7 @@ if not fc: print("L6 messsage file cannot open.") sys.exit(-1) + for ne in range(nep): obs = dec.decode_obs() week, tow = time2gpst(obs.t) @@ -59,12 +62,14 @@ cstat = cs.chk_stat() if cstat: + if ne>=42: + ne ppprtkpos(nav, obs, cs) t[ne] = timediff(nav.t, t0) tc[ne] = timediff(cs.time, t0) - sol = nav.x[0:3] + sol = nav.xa[0:3] if nav.smode == 4 else nav.x[0:3] enu[ne, :] = gn.ecef2enu(pos_ref, sol-xyz_ref) smode[ne] = nav.smode diff --git a/cssrlib/tests/test_ppprtk2.py b/cssrlib/tests/test_ppprtk2.py new file mode 100644 index 0000000..b157bf4 --- /dev/null +++ b/cssrlib/tests/test_ppprtk2.py @@ -0,0 +1,115 @@ +import matplotlib.pyplot as plt +import numpy as np +from cssrlib.cssrlib import cssr +import cssrlib.rinex as rn +import cssrlib.gnss as gn +from cssrlib.ppprtk import rtkinit, ppprtkpos + +navfile = '../data/SEPT2650.21P' +obsfile = '../data/SEPT265G.21O' +#l6file = '../data/2021265G.l6' +l6file = 'c:/work/log/archive/KB4/2021265G.l6' +griddef = '../data/clas_grid.def' + +xyz_ref = gn.pos2ecef([35.342058098, 139.521986657, 47.5515], True) + +cs = cssr() +cs.monlevel = 2 +cs.week = 2176 # 2021/9/22 +cs.read_griddef(griddef) + +# rover +dec = rn.rnxdec() +nav = gn.Nav() +dec.decode_nav(navfile, nav) +dec.decode_obsh(obsfile) + +nep = 360 +# nav.excl_sat = [158] +t = np.zeros(nep) +enu = np.zeros((nep, 3)) +smode = np.zeros(nep, dtype=int) +rr0 = [-3961951.1326752, 3381198.11019757, 3668916.0417232] # from pntpos +# rr0 = xyz_ref +pos_ref = gn.ecef2pos(xyz_ref) + +# nav.elmin = np.deg2rad(30.0) + +if True: + rtkinit(nav, rr0) + pos = gn.ecef2pos(rr0) + inet = cs.find_grid_index(pos) + + fc = open(l6file, 'rb') + if not fc: + print("L6 messsage file cannot open.") + exit(-1) + + # t_obs 06:29:30 + fc.seek(250*(29*60+30)) # seek to 06:29:30 + if True: + for k in range(30): # read 30 sec + cs.decode_l6msg(fc.read(250), 0) + if cs.fcnt == 5: # end of sub-frame + cs.decode_cssr(cs.buff, 0) + + for ne in range(nep): + obs = dec.decode_obs() + week, tow = gn.time2gpst(obs.t) + + cs.decode_l6msg(fc.read(250), 0) + if cs.fcnt == 5: # end of sub-frame + cs.week = week + cs.decode_cssr(cs.buff, 0) + + if ne == 0: + t0 = nav.t = obs.t + cs.time = obs.t + nav.time_p = t0 + + if ne >= 59: + ne + cstat = cs.chk_stat() + + if cstat: + ppprtkpos(nav, obs, cs) + t[ne] = gn.timediff(nav.t, t0) + sol = nav.xa[0:3] if nav.smode==4 else nav.x[0:3] + enu[ne, :] = gn.ecef2enu(pos_ref, sol-xyz_ref) + smode[ne] = nav.smode + + dec.fobs.close() + +if True: + idx4 = np.where(smode == 4)[0] + idx5 = np.where(smode == 5)[0] + idx1 = np.where(smode == 1)[0] + + lbl_t = ['east [m]', 'north [m]', 'up [m]'] + fig = plt.figure(figsize=(6, 10)) + + for k in range(3): + plt.subplot(3, 1, k+1) + plt.plot(t, enu[:, k], '-', color='gray') + # plt.plot(t[idx1], enu[idx1, k], 'm.', label='stdpos') + plt.plot(t[idx5], enu[idx5, k], 'g.', markersize=1, label='float') + plt.plot(t[idx4], enu[idx4, k], 'b.', markersize=1, label='fix') + plt.xticks(np.arange(0, nep+1, step=30)) + plt.ylabel(lbl_t[k]) + plt.xlabel('time[s]') + if k == 0: + plt.legend() + plt.grid() + plt.show() + + plt.plot(enu[:, 0], enu[:, 1], '-', color='gray') + # plt.plot(enu[idx1, 0], enu[idx1, 1], 'm.', markersize=1, label='stdpos') + plt.plot(enu[idx5, 0], enu[idx5, 1], 'g.', markersize=1, label='float') + plt.plot(enu[idx4, 0], enu[idx4, 1], 'b.', markersize=1, label='fix') + + plt.xlabel('easting [m]') + plt.ylabel('northing [m]') + plt.grid() + plt.axis('equal') + plt.legend() + plt.show() diff --git a/cssrlib/tests/test_rtk.py b/cssrlib/tests/test_rtk.py index a61e528..2b377f4 100644 --- a/cssrlib/tests/test_rtk.py +++ b/cssrlib/tests/test_rtk.py @@ -43,11 +43,9 @@ obs, obsb = rn.sync_obs(dec, decb) if ne == 0: t0 = nav.t = obs.t - if ne>=18: - print(ne) relpos(nav, obs, obsb) t[ne] = gn.timediff(nav.t, t0) - sol = nav.xa[0:3] + sol = nav.xa[0:3] if nav.smode == 4 else nav.x[0:3] enu[ne, :] = gn.ecef2enu(pos_ref, sol-xyz_ref) smode[ne] = nav.smode diff --git a/cssrlib/tests/test_rtk2.py b/cssrlib/tests/test_rtk2.py index 6b6b8a4..68f0242 100644 --- a/cssrlib/tests/test_rtk2.py +++ b/cssrlib/tests/test_rtk2.py @@ -38,7 +38,7 @@ t0 = nav.t = obs.t relpos(nav, obs, obsb) t[ne] = gn.timediff(nav.t, t0) - sol = nav.x[0:3] + sol = nav.xa[0:3] if nav.smode == 4 else nav.x[0:3] enu[ne, :] = gn.ecef2enu(pos_ref, sol-xyz_ref) smode[ne] = nav.smode diff --git a/setup.py b/setup.py index c62dc8c..e082dba 100644 --- a/setup.py +++ b/setup.py @@ -5,7 +5,7 @@ setuptools.setup( name="cssrlib", - version="0.0.1", + version="0.0.3", install_requires=[ "matplotlib", "numpy", @@ -20,13 +20,13 @@ long_description_content_type="text/markdown", url="https://github.com/hirokawa/cssrlib", packages=setuptools.find_packages(), - package_data={ - "cssrlib": ["data/*.*", "tests/*.py"], - }, + #package_data={ + # "cssrlib": ["data/*.*", "tests/*.py"], + #}, classifiers=[ "Programming Language :: Python :: 3", "License :: OSI Approved :: MIT License", "Operating System :: OS Independent", ], - python_requires='>=3.8', + python_requires='>=3.9', )