Skip to content

Commit

Permalink
- added monitor
Browse files Browse the repository at this point in the history 
- kinematic case test for ppp-rtk.
  • Loading branch information
@hirokawa
hirokawa committed Dec 19, 2021
1 parent 1a8b8f5 commit 2e4cda3
Show file tree
Hide file tree
Showing 9 changed files with 202 additions and 61 deletions.
6 changes: 3 additions & 3 deletions cssrlib/cssrlib.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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)
Expand Down Expand Up @@ -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']
Expand Down
2 changes: 1 addition & 1 deletion cssrlib/gnss.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down
111 changes: 67 additions & 44 deletions cssrlib/ppprtk.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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


Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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):
Expand All @@ -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):
Expand All @@ -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:
Expand Down Expand Up @@ -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]
Expand Down Expand Up @@ -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_)
Expand All @@ -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):
Expand All @@ -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
Expand All @@ -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)

Expand Down Expand Up @@ -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
Expand Down
4 changes: 2 additions & 2 deletions cssrlib/rtk.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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, :]
Expand Down
9 changes: 7 additions & 2 deletions cssrlib/tests/test_ppprtk.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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)
Expand All @@ -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

Expand Down
Loading

0 comments on commit 2e4cda3

Please sign in to comment.