- added SBAS DFMC signal selection

- added partial AR (experimental)

Author: hirokawa

src/cssrlib/mlambda.py

@@ -11,6 +11,8 @@
 """
 
 import numpy as np
+from scipy.stats import norm
+from numpy.linalg import inv
 
 
 def ldldecom(Q):
@@ -66,6 +68,12 @@ def reduction(L, d):
     return L, d, Z
 
 
+def sr_boost(d):
+    """ Compute the bootstrapped success rate """
+    Ps = np.prod(2*norm.cdf(0.5/np.sqrt(d))-1)
+    return Ps
+
+
 def msearch(L, d, zs, m=2):
     n = len(d)
     nn = 0
@@ -129,14 +137,67 @@ def msearch(L, d, zs, m=2):
     return zn, s
 
 
-def mlambda(a, Q, m=2):
-    L, d = ldldecom(Q)
+def parsearch(zhat, Qzhat, Z, L, d, P0=0.995, ncands=2):
+    """ Partial Ambiguity Resolution """
+    n = len(Qzhat)
+    Ps = sr_boost(d)
+
+    k = 0
+    while Ps < P0 and k < (n-1):
+        k += 1
+
+        # Compute the bootstrapped success rate if the last n-k+1 ambiguities
+        # would be fixed
+        Ps = sr_boost(d[k:])
+
+    if Ps > P0:
+
+        zpar, sqnorm = msearch(L[k:, k:], d[k:], zhat[k:], ncands)
+
+        Qzpar = Qzhat[k:, k:]
+        Zpar = Z[:, k:]
+
+        # First k-1 ambiguities are adjusted based on correlation with the
+        # fixed ambiguities
+        QP = Qzhat[:k, k:]@inv(Qzhat[k:, k:])
+
+        zfix = np.zeros((k, ncands))
+        for i in range(ncands):
+            zfix[:, i] = zhat[:k] - QP@(zhat[k:]-zpar[:, i])
+
+        zfix = np.concatenate((zfix, zpar), axis=0)
+        nfix = n-k
+
+    else:
+
+        zpar = []
+        Qzpar = []
+        Zpar = []
+        sqnorm = []
+        Ps = np.nan
+        zfix = zhat
+        nfix = 0
+
+    return zpar, sqnorm, Qzpar, Zpar, Ps, nfix, zfix
+
+
+def mlambda(ahat, Qahat, ncands=2, armode=1, P0=0.995):
+    """ modified LAMBDA method for ambiguity resolution """
+    L, d = ldldecom(Qahat)
     L, d, Z = reduction(L, d)
-    invZt = np.round(np.linalg.inv(Z.T))
-    z = Z.T@a
-    E, s = msearch(L, d, z, m)
-    afix_ = invZt@E
-    return afix_, s
+    iZt = np.round(inv(Z.T))
+    zhat = Z.T@ahat
+
+    if armode == 1:
+        zfix, s = msearch(L, d, zhat, ncands)
+        Ps, nfix = np.nan, len(zhat)
+    elif armode == 2:  # PAR
+        Qzhat = Z.T@Qahat@Z
+        zpar, s, Qzpar, Zpar, Ps, nfix, zfix = parsearch(zhat, Qzhat, Z, L, d,
+                                                         P0, ncands)
+
+    afix_ = iZt@zfix
+    return afix_, s, nfix, Ps
 
 
 if __name__ == '__main__':

src/cssrlib/pppssr.py

@@ -121,6 +121,9 @@ def __init__(self, nav, pos0=np.zeros(3),
         self.nav.elmaskar = np.deg2rad(20.0)  # elevation mask for AR
         self.nav.elmin = np.deg2rad(15.0)
 
+        self.nav.parmode = 2  # 1: normal, 2: PAR
+        self.nav.par_P0 = 0.995  # probability of sussefull AR
+
         # Initial state vector
         #
         self.nav.x[0:3] = pos0
@@ -657,11 +660,13 @@ def zdres(self, obs, cs, bsx, rs, vs, dts, rr, rtype=1):
                     elif sys == uGNSS.BDS:
                         sig0 = (rSigRnx("CC6I"),)
 
-                elif cs.cssrmode == sc.PVS_PPP:
+                elif cs.cssrmode in (sc.PVS_PPP, sc.SBAS_L1, sc.SBAS_L5):
                     if sys == uGNSS.GPS:
                         sig0 = (rSigRnx("GC1C"), rSigRnx("GC5Q"))
                     elif sys == uGNSS.GAL:
                         sig0 = (rSigRnx("EC1C"), rSigRnx("EC5Q"))
+                    elif sys == uGNSS.SBS:
+                        sig0 = (rSigRnx("SC1C"), rSigRnx("SC5Q"))
 
             # Receiver/satellite antenna offset
             #
@@ -1054,7 +1059,7 @@ def ddidx(self, nav, sat):
         ix = np.resize(ix, (nb, 2))
         return ix
 
-    def resamb_lambda(self, sat):
+    def resamb_lambda(self, sat, armode=1, P0=0.995):
         """ resolve integer ambiguity using LAMBDA method """
         nx = self.nav.nx
         na = self.nav.na
@@ -1072,8 +1077,9 @@ def resamb_lambda(self, sat):
         Qab = self.nav.P[0:na, ix[:, 0]]-self.nav.P[0:na, ix[:, 1]]
 
         # MLAMBDA ILS
-        b, s = mlambda(y, Qb)
-        if s[0] <= 0.0 or s[1]/s[0] >= self.nav.thresar:
+        b, s, nfix, Ps = mlambda(y, Qb, armode=armode, P0=P0)
+        if s[0] <= 0.0 or s[1]/s[0] >= self.nav.thresar \
+                or (armode == 2 and nfix > 0):
             self.nav.xa = self.nav.x[0:na].copy()
             self.nav.Pa = self.nav.P[0:na, 0:na].copy()
             bias = b[:, 0]
@@ -1084,6 +1090,13 @@ def resamb_lambda(self, sat):
 
             # restore SD ambiguity
             xa = self.restamb(bias, nb)
+
+        elif armode == 2 and nfix == 0:
+            nb = 0
+            if self.nav.monlevel > 0:
+                self.nav.fout.write(
+                    "{:s}  Ps={:3.2f} nfix={:d}\n".
+                    format(time2str(self.time), Ps, nfix))
         else:
             nb = 0
 
@@ -1404,7 +1417,7 @@ def process(self, obs, cs=None, orb=None, bsx=None, obsb=None):
         self.nav.smode = 5  # 4: fixed ambiguities, 5: float ambiguities
 
         if self.nav.armode > 0:
-            nb, xa = self.resamb_lambda(sat)
+            nb, xa = self.resamb_lambda(sat, self.nav.parmode, self.nav.par_P0)
             if nb > 0:
                 # Use position with fixed ambiguities xa
                 yu, eu, elu = self.zdres(obs, cs, bsx, rs, vs, dts, xa[0:3])
@@ -1416,6 +1429,10 @@ def process(self, obs, cs=None, orb=None, bsx=None, obsb=None):
                     if self.nav.armode == 3:     # fix and hold
                         self.holdamb(xa)    # hold fixed ambiguity
                     self.nav.smode = 4           # fix
+                else:
+                    pass
+            else:
+                pass
 
         # Store epoch for solution
         #