Skip to content

Commit 68ecf73

Browse files
hirokawahirokawa
committed
- align with new Lambda toolbox [4]
1 parent db415a9 commit 68ecf73

File tree

1 file changed

+177
-65
lines changed

1 file changed

+177
-65
lines changed

‎src/cssrlib/mlambda.py‎

Lines changed: 177 additions & 65 deletions
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,21 @@
11
"""
22
integer ambiguity resolution by LAMBDA
33
4-
reference :
4+
reference:
55
[1] P.J.G.Teunissen, The least-square ambiguity decorrelation adjustment:
66
a method for fast GPS ambiguity estimation, J.Geodesy, Vol.70, 65-82,
77
1995
88
[2] X.-W.Chang, X.Yang, T.Zhou, MLAMBDA: A modified LAMBDA method for
99
integer least-squares estimation, J.Geodesy, Vol.79, 552-565, 2005
1010
11+
[3] A. Ghasemmehdi and E. Agrell, Faster Recursions in Sphere
12+
Decoding, IEEE Transactions on Information Theory, vol. 57,
13+
no. 6, pp. 3530-3536, June 2011.
14+
15+
[4] Massarweh, L., Verhagen, S., and Teunissen, P.J.G.
16+
New LAMBDA toolbox for mixed-integer models:
17+
estimation and evaluation, GPS Solution, 29, 14, 2025.
18+
1119
"""
1220

1321
import numpy as np
@@ -16,19 +24,20 @@
1624

1725

1826
def ldldecom(Q):
27+
""" Lt*d*L decomposition of positive symmetric matrix Q """
1928
n = len(Q)
2029
L = np.zeros((n, n))
2130
d = np.zeros(n)
22-
A = Q.copy()
2331
for i in range(n-1, -1, -1):
24-
d[i] = A[i, i]
25-
if d[i] <= 0.0:
26-
raise SystemExit("Qah should be positive definite.")
27-
L[i, :i+1] = A[i, :i+1]/np.sqrt(d[i])
32+
d[i] = Q[i, i].copy()
33+
L[i, :i+1] = Q[i, :i+1]/np.sqrt(Q[i, i])
2834
for j in range(i):
29-
A[j, :j+1] -= L[i, :j+1]*L[i, j]
35+
Q[j, :j+1] -= L[i, :j+1]*L[i, j]
3036
L[i, :i+1] /= L[i, i]
3137

38+
if np.any((d < 1e-10)):
39+
raise SystemExit("Qah should be positive definite.")
40+
3241
return L, d
3342

3443

@@ -44,7 +53,7 @@ def reduction(L, d):
4453
if mu != 0.0:
4554
L[i:, j] -= mu*L[i:, i]
4655
Z[:, j] -= mu*Z[:, i]
47-
# L,Z=gauss(L,Z,i,j)
56+
4857
delta = d[j]+L[j+1, j]**2*d[j+1]
4958
if delta+1e-6 < d[j+1]: # permutation
5059
eta = d[j]/delta
@@ -54,12 +63,8 @@ def reduction(L, d):
5463
L[j:j+2, :j] = np.array([[-L[j+1, j], 1], [eta, lam]])@L[j:j+2, :j]
5564
L[j+1, j] = lam
5665
# swap j,j+1 row
57-
tmp = L[j+2:, j+1].copy()
58-
L[j+2:, j+1] = L[j+2:, j].copy()
59-
L[j+2:, j] = tmp
60-
tmp = Z[:, j+1].copy()
61-
Z[:, j+1] = Z[:, j].copy()
62-
Z[:, j] = tmp
66+
L[j+2:, [j+1, j]] = L[j+2:, [j, j+1]]
67+
Z[:, [j+1, j]] = Z[:, [j, j+1]]
6368

6469
k = j
6570
j = n-2
@@ -74,74 +79,171 @@ def sr_boost(d):
7479
return Ps
7580

7681

77-
def msearch(L, d, zs, m=2):
82+
def msearch(L, d, ahat, ncands=2):
7883
n = len(d)
79-
nn = 0
80-
imax = 0
8184
Chi2 = 1e18
82-
S = np.zeros((n, n))
85+
8386
dist = np.zeros(n)
84-
zb = np.zeros(n)
85-
z = np.zeros(n)
86-
step = np.zeros(n)
87-
zn = np.zeros((n, m))
88-
s = np.zeros(m)
89-
k = n-1
90-
zb[-1] = zs[-1]
91-
z[-1] = round(zb[-1])
92-
y = zb[-1]-z[-1]
93-
step[-1] = np.sign(y)
87+
acond = np.zeros(n)
88+
zcond = np.zeros(n, dtype=np.int32)
89+
step = np.zeros(n, dtype=np.int32)
90+
afixed = np.zeros((n, ncands))
91+
sqnorm = np.zeros(ncands)
92+
93+
acond[-1] = ahat[-1]
94+
zcond[-1] = round(acond[-1])
95+
left = acond[-1]-zcond[-1]
96+
step[-1] = np.sign(left)
9497
if step[-1] == 0:
9598
step[-1] = 1
96-
for _ in range(10000):
97-
newdist = dist[k]+y**2/d[k]
99+
100+
imax = ncands - 1
101+
S = np.zeros((n, n))
102+
count = -1 # number of candidates
103+
endSearch = False
104+
k = n-1
105+
106+
while not endSearch:
107+
newdist = dist[k]+left**2/d[k]
98108
if newdist < Chi2:
99109
if k != 0:
100110
k -= 1
101111
dist[k] = newdist
102-
S[k, :k+1] = S[k+1, :k+1]+(z[k+1]-zb[k+1])*L[k+1, :k+1]
103-
zb[k] = zs[k]+S[k, k]
104-
z[k] = round(zb[k])
105-
y = zb[k]-z[k]
106-
step[k] = np.sign(y)
112+
S[k, :k+1] = S[k+1, :k+1]+(zcond[k+1]-acond[k+1])*L[k+1, :k+1]
113+
acond[k] = ahat[k]+S[k, k]
114+
zcond[k] = round(acond[k])
115+
left = acond[k]-zcond[k]
116+
step[k] = np.sign(left)
107117
if step[k] == 0:
108118
step[k] = 1
109119
else:
110-
if nn < m:
111-
if nn == 0 or newdist > s[imax]:
112-
imax = nn
113-
zn[:, nn] = z
114-
s[nn] = newdist
115-
nn += 1
120+
if count < ncands-2:
121+
count += 1
122+
afixed[:, count] = zcond
123+
sqnorm[count] = newdist
116124
else:
117-
if newdist < s[imax]:
118-
zn[:, imax] = z
119-
s[imax] = newdist
120-
imax = np.argmax(s)
121-
Chi2 = s[imax]
122-
z[0] += step[0]
123-
y = zb[0]-z[0]
125+
afixed[:, imax] = zcond
126+
sqnorm[imax] = newdist
127+
128+
imax = np.argmax(sqnorm)
129+
Chi2 = sqnorm[imax]
130+
131+
zcond[0] += step[0]
132+
left = acond[0]-zcond[0]
124133
step[0] = -step[0]-np.sign(step[0])
125134
else:
126135
if k == n-1:
136+
endSearch = True
137+
else:
138+
k += 1
139+
zcond[k] += step[k]
140+
left = acond[k]-zcond[k]
141+
step[k] = -step[k]-np.sign(step[k])
142+
143+
order = np.argsort(sqnorm)
144+
sqnorm = sqnorm[order]
145+
afixed = afixed[:, order]
146+
147+
return afixed, sqnorm
148+
149+
150+
def estimILS(L, d, ahat, ncands=2):
151+
""" ILS estimator by search-and-shrink [4] """
152+
n = len(d)
153+
Chi2 = 1e18
154+
155+
k0 = 1 if (ncands == 1 and n > 1) else 0
156+
157+
afixed = np.zeros((n, ncands))
158+
sqnorm = np.zeros(ncands)
159+
160+
acond = np.zeros(n)
161+
zcond = np.zeros(n, dtype=np.int32)
162+
left = np.zeros(n)
163+
step = np.zeros(n, dtype=np.int32)
164+
165+
acond[-1] = ahat[-1]
166+
zcond[-1] = round(acond[-1])
167+
left[-1] = acond[-1] - zcond[-1]
168+
step[-1] = np.sign(left[-1])
169+
if step[-1] == 0:
170+
step[-1] = 1
171+
172+
count = -1 # number of candidates
173+
imax = ncands - 1
174+
175+
S = np.zeros((n, n))
176+
dist = np.zeros(n)
177+
path = (n-1)*np.ones(n, dtype=np.int32)
178+
179+
endSearch = False
180+
k = n-1
181+
182+
while not endSearch:
183+
newdist = dist[k]+left[k]**2/d[k]
184+
while newdist < Chi2:
185+
if k != 0:
186+
k -= 1
187+
dist[k] = newdist
188+
189+
for j in range(path[k], k, -1):
190+
S[j-1, k] = S[j, k]-left[j]*L[j, k]
191+
192+
acond[k] = ahat[k]+S[k, k]
193+
zcond[k] = round(acond[k])
194+
left[k] = acond[k]-zcond[k]
195+
step[k] = np.sign(left[k])
196+
if step[k] == 0:
197+
step[k] = 1
198+
else:
199+
if count < ncands-2:
200+
count += 1
201+
afixed[:, count] = zcond
202+
sqnorm[count] = newdist
203+
else:
204+
afixed[:, imax] = zcond
205+
sqnorm[imax] = newdist
206+
207+
imax = np.argmax(sqnorm)
208+
Chi2 = sqnorm[imax]
209+
210+
# next valid integer (k+1 level)
211+
k = k0
212+
zcond[k] += step[k]
213+
left[k] = acond[k]-zcond[k]
214+
step[k] = -step[k]-np.sign(step[k])
215+
216+
newdist = dist[k] + left[k]**2/d[k]
217+
218+
ilevel = k
219+
220+
while newdist >= Chi2:
221+
if k == n-1:
222+
endSearch = True
127223
break
128224
k += 1
129-
z[k] += step[k]
130-
y = zb[k]-z[k]
225+
zcond[k] += step[k]
226+
left[k] = acond[k]-zcond[k]
131227
step[k] = -step[k]-np.sign(step[k])
228+
newdist = dist[k] + left[k]**2/d[k]
229+
230+
path[ilevel:k] = k
231+
for j in range(ilevel-1, -1, -1):
232+
if path[j] < k:
233+
path[j] = k
234+
else:
235+
break
132236

133-
order = np.argsort(s)
134-
s = s[order]
135-
zn = zn[:, order]
237+
order = np.argsort(sqnorm)
238+
sqnorm = sqnorm[order]
239+
afixed = afixed[:, order]
136240

137-
return zn, s
241+
return afixed, sqnorm
138242

139243

140-
def parsearch(zhat, Qzhat, Z, L, d, P0=0.995, ncands=2):
244+
def parsearch(zhat, Qzhat, Z, L, d, Ps, P0=0.995, ncands=2):
141245
""" Partial Ambiguity Resolution """
142246
n = len(Qzhat)
143-
Ps = sr_boost(d)
144-
145247
k = 0
146248
while Ps < P0 and k < (n-1):
147249
k += 1
@@ -152,7 +254,8 @@ def parsearch(zhat, Qzhat, Z, L, d, P0=0.995, ncands=2):
152254

153255
if Ps > P0:
154256

155-
zpar, sqnorm = msearch(L[k:, k:], d[k:], zhat[k:], ncands)
257+
# zpar, sqnorm = msearch(L[k:, k:], d[k:], zhat[k:], ncands)
258+
zpar, sqnorm = estimILS(L[k:, k:], d[k:], zhat[k:], ncands)
156259

157260
Qzpar = Qzhat[k:, k:]
158261
Zpar = Z[:, k:]
@@ -183,25 +286,34 @@ def parsearch(zhat, Qzhat, Z, L, d, P0=0.995, ncands=2):
183286

184287
def mlambda(ahat, Qahat, ncands=2, armode=1, P0=0.995):
185288
""" modified LAMBDA method for ambiguity resolution """
289+
# s = time.perf_counter_ns()
186290
L, d = ldldecom(Qahat)
291+
# e = time.perf_counter_ns()
292+
# t1_ = (e-s)*1e-9
293+
187294
L, d, Z = reduction(L, d)
188295
iZt = np.round(inv(Z.T))
189296
zhat = Z.T@ahat
297+
Qzhat = L.T@np.diag(d)@L
298+
299+
Ps = sr_boost(d)
190300

191301
if armode == 1:
192-
zfix, s = msearch(L, d, zhat, ncands)
193-
Ps, nfix = np.nan, len(zhat)
302+
# zfix, s = msearch(L, d, zhat, ncands)
303+
zfix, s = estimILS(L, d, zhat, ncands)
304+
nfix = len(zhat)
305+
194306
elif armode == 2: # PAR
195-
Qzhat = Z.T@Qahat@Z
196307
zpar, s, Qzpar, Zpar, Ps, nfix, zfix = parsearch(zhat, Qzhat, Z, L, d,
197-
P0, ncands)
308+
Ps, P0, ncands)
198309

199310
afix_ = iZt@zfix
200311
return afix_, s, nfix, Ps
201312

202313

203314
if __name__ == '__main__':
204-
ncase = 1
315+
ncase = 2
316+
armode = 1
205317

206318
if ncase == 1:
207319
Qah = np.array([[6.2900, 5.9780, 0.5440], [
@@ -213,7 +325,7 @@ def mlambda(ahat, Qahat, ncands=2, armode=1, P0=0.995):
213325
14858.8484050976, -12299.1993741839, -13507.1694819930,
214326
11230.0704356810, 7835.62344938376, -11111.1393808147],
215327
[-15783.9722820370, 59027.7038409815, 38142.6927531102,
216-
.717388024645, -13830.0855960676, 27373.4263013019,
328+
562.717388024645, -13830.0855960676, 27373.4263013019,
217329
-12299.1993747356, 45995.6129934030, 29721.5785731468,
218330
438.480887460148, -10776.6902686912, 21329.9423774758],
219331
[-17334.2005875975, 38142.6927531102, 28177.5653893528,
@@ -263,4 +375,4 @@ def mlambda(ahat, Qahat, ncands=2, armode=1, P0=0.995):
263375
3899.40332138829, -22749.1853575113, -159.278779870217]
264376
ah = np.array(ah)
265377

266-
afix, sqnorm = mlambda(ah, Qah)
378+
afix, sqnorm, nfix, Ps = mlambda(ah, Qah, armode=armode, P0=0.95)

0 commit comments

Comments
 (0)