1 /* Copyright 2002-2025 CS GROUP
2 * Licensed to CS GROUP (CS) under one or more
3 * contributor license agreements. See the NOTICE file distributed with
4 * this work for additional information regarding copyright ownership.
5 * CS licenses this file to You under the Apache License, Version 2.0
6 * (the "License"); you may not use this file except in compliance with
7 * the License. You may obtain a copy of the License at
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17 package org.orekit.estimation.measurements.gnss;
18
19 import org.hipparchus.util.FastMath;
20
21 /** Decorrelation/reduction engine for Modified LAMBDA method.
22 * <p>
23 * This class implements Modified Least Square Ambiguity Decorrelation
24 * Adjustment (MLAMBDA) method, as described in <a
25 * href="https://www.researchgate.net/publication/225518977_MLAMBDA_a_modified_LAMBDA_method_for_integer_least-squares_estimation">
26 * A modified LAMBDA method for integer least-squares estimation</a> by X.-W Chang, X. Yang
27 * and T. Zhou, Journal of Geodesy 79(9):552-565, DOI: 10.1007/s00190-005-0004-x
28 * </p>
29 *
30 * @see AmbiguitySolver
31 * @author David Soulard
32 * @since 10.2
33 */
34 public class ModifiedLambdaMethod extends AbstractLambdaMethod {
35
36 /** Empty constructor.
37 * <p>
38 * This constructor is not strictly necessary, but it prevents spurious
39 * javadoc warnings with JDK 18 and later.
40 * </p>
41 * @since 12.0
42 */
43 public ModifiedLambdaMethod() {
44 // nothing to do
45 }
46
47 /** Compute the LᵀDL factorization with symmetric pivoting decomposition of Q
48 * (symmetric definite positive matrix) with a minimum symmetric pivoting: Q = ZᵀLᵀDLZ.
49 */
50 @Override
51 protected void ltdlDecomposition() {
52 final double[] diag = getDiagReference();
53 final double[] low = getLowReference();
54 final int[] Z = getZInverseTransformationReference();
55 final double[] aNew = getDecorrelatedReference();
56 for (int i = 0; i < this.getSize(); i++) {
57 Z[zIndex(i, i)] = 0;
58 }
59 final int n = diag.length;
60 final int[] perm = new int[n];
61 for (int i = 0; i < n; i++) {
62 perm[i] = i;
63 }
64 for (int k = n - 1; k >= 0; k--) {
65 final int q = posMin(diag, k);
66 exchangeIntP1WithIntP2(perm, k, q);
67 permRowThenCol(low, diag, k, q);
68 if (k > 0) {
69 final double Wkk = diag[k];
70 divide(low, Wkk, k);
71 set(low, diag, k);
72 }
73 exchangeDoubleP1WithDoubleP2(aNew, k, q);
74 }
75 for (int j = 0; j < n; j++) {
76 Z[zIndex(j, perm[j])] = 1;
77 }
78 }
79
80 /** Perform MLAMBDA reduction.
81 */
82 @Override
83 protected void reduction() {
84 final double[] diag = getDiagReference();
85 final double[] low = getLowReference();
86 final int n = diag.length;
87 int k = getSize() - 2;
88 while ( k > -1) {
89 final int kp1 = k + 1;
90 double tmp = low[lIndex(kp1, k)];
91 final double mu = FastMath.rint(tmp);
92 if (Math.abs(mu) > 1e-9) {
93 tmp -= mu;
94 }
95 final double delta = diag[k] + tmp * tmp * diag[kp1];
96 if (delta < diag[kp1]) {
97 integerGaussTransformation(kp1, k);
98 if (mu > 0) {
99 for (int i = k + 2; i < n; i++) {
100 integerGaussTransformation(i, k);
101 }
102 }
103 permutation(k, delta);
104 if (k < n - 2) {
105 k++;
106 }
107 } else {
108 k--;
109 }
110 }
111 }
112
113 /** {@inheritDoc} */
114 @Override
115 protected void discreteSearch() {
116 //initialization
117 final int n = getSize();
118 final int maxSolutions = getMaxSolution();
119 final double[] diag = getDiagReference();
120 final double[] decorrelated = getDecorrelatedReference();
121 final double[] low = getLowReference();
122 final long[] z = new long[n];
123 final double[] zb = new double[n];
124 final double[] y = new double[n];
125 final int[] path = new int[n];
126
127 for (int i = 0; i < n; i++) {
128 path[i] = n - 1;
129 }
130
131 final double[] step = new double[n];
132 final double[] dist = new double[n + 1];
133 final double[] lS = new double[(n * (n - 1)) / 2];
134 final double[] dS = new double[n];
135 double maxDist = Double.POSITIVE_INFINITY;
136 int count = 0;
137 boolean endSearch = false;
138 int ulevel = 0;
139
140 // Determine which level to move to after z(0) is chosen at level 1.
141 final int k0;
142 if (maxSolutions == 1) {
143 k0 = 1;
144 }
145 else {
146 k0 = 0;
147 }
148
149 // Initialization at level n
150 zb[n - 1] = decorrelated.clone()[n - 1];
151 z[n - 1] = (long) FastMath.rint(zb[n - 1]);
152 y[n - 1] = zb[n - 1] - z[n - 1];
153 step[n - 1] = sign(y[n - 1]);
154 int k = n - 1;
155 while (!endSearch) {
156 for (int i = ulevel; i <= k - 1; i++) {
157 path[i] = k;
158 }
159 for (int j = ulevel - 1; j >= 0; j--) {
160 if (path[j] < k) {
161 path[j] = k;
162 } else {
163 break;
164 }
165 }
166 double newDist = dist[k] + y[k] * y[k] / diag[k];
167 while (newDist < maxDist) {
168 // move to level k-1
169 if (k != 0) {
170 k--;
171 dist[k] = newDist;
172 for (int j = path[k]; j > k; j--) {
173 if (j - 1 == k) {
174 //Update diagonal
175 dS[k] = lS[lIndex(j, k)] - y[j] * low[lIndex(j, k)];
176 } else {
177 //Update low triangular part
178 lS[lIndex(j - 1, k)] = lS[lIndex(j, k)] - y[j] * low[lIndex(j, k)];
179 }
180 }
181
182 zb[k] = decorrelated[k] + dS[k];
183 z[k] = (long) FastMath.rint(zb[k]);
184 y[k] = zb[k] - z[k];
185 step[k] = sign(y[k]);
186 } else {
187 //Save the value of one optimum z
188 if (count < (maxSolutions - 1)) {
189 addSolution(z, newDist);
190 count++;
191 //Save the value of one optimum z
192 } else if (count == (maxSolutions - 1)) {
193 addSolution(z, newDist);
194 maxDist = getMaxDistance();
195 count++;
196 //Replace the new solution with the one with the greatest distance
197 } else {
198 removeSolution();
199 addSolution(z, newDist);
200 maxDist = getMaxDistance();
201 }
202 k = k0;
203 z[k] = z[k] + (long) step[k];
204 y[k] = zb[k] - z[k];
205 step[k] = -step[k] - sign(step[k]);
206 }
207 newDist = dist[k] + y[k] * y[k] / diag[k];
208 }
209 ulevel = k;
210 //exit or move to level k+1
211 while (newDist >= maxDist) {
212 if (k == n - 1) {
213 endSearch = true;
214 break;
215 }
216 //move to level k+1
217 k++;
218 //next integer
219 z[k] = z[k] + (long) step[k];
220 y[k] = zb[k] - z[k];
221 step[k] = -step[k] - sign(step[k]);
222 newDist = dist[k] + y[k] * y[k] / diag[k];
223 }
224 }
225 }
226
227 /** {@inheritDoc} */
228 @Override
229 protected void inverseDecomposition() {
230 // nothing for M-LAMBDA method
231 }
232
233 /** Return the position of the smallest element in the diagonal of a matrix given in parameter.
234 * @param k the position of the smallest diagonal element
235 * @param D an array of double being the diagonal of the covariance matrix
236 * @return the position of the smallest element of mat.
237 */
238 private int posMin(final double[] D, final int k) {
239 int q = 0;
240 double value = D[0];
241 for (int i = 1; i <= k; i++) {
242 if (value > D[i]) {
243 value = D[i];
244 q = i;
245 }
246 }
247 return q;
248 }
249
250 /** Perform the following operation on the matrix W in parameters.
251 * W(1:p-1,1:p-1) = W(1:p-1,1:p-1) - W(p,1:p-1)'*W(p,p)*W(p,1:p-1);
252 * @param L array of double being a lower triangular part of the covariance matrix
253 * @param D array of double being the diagonal of the covariance matrix
254 * @param p integer at which the computation is done
255 */
256 private void set(final double[] L, final double[] D, final int p) {
257 final double d = D[p];
258 final double[] row = new double[p];
259 for (int i = 0; i < p; i++) {
260 row[i] = L[lIndex(p, i)];
261 }
262 for (int i = 0; i < p; i++) {
263 for (int j = 0; j < i; j++) {
264 L[lIndex(i, j)] = L[lIndex(i, j)] - row[i] * row[j] * d;
265 }
266 D[i] = D[i] - row[i] * row[i] * d;
267 }
268 }
269
270 /** Perform a permutation between two row and then two column of the covariance matrix.
271 * @param L array of double being the lower triangular part of the covariance matrix
272 * @param D array of double being the diagonal of the covariance matrix
273 * @param p1 integer: position of the permutation
274 * @param p2 integer: position of the permutation
275 */
276 private void permRowThenCol(final double[] L, final double[] D, final int p1, final int p2) {
277 final double[] rowP1 = getRow(L, D, p1);
278 final double[] rowP2 = getRow(L, D, p2);
279 if (p1 > p2) {
280 //Update row P2
281 for (int j = 0; j < p2; j++) {
282 L[lIndex(p2, j)] = rowP1[j];
283 }
284 D[p2] = rowP1[p2];
285 //Update row p1
286 for (int j = 0; j < p1; j++) {
287 L[lIndex(p1, j)] = rowP2[j];
288 }
289 D[p1] = rowP2[p1];
290 final double[] colP1 = getColPmax(L, D, rowP1, p2, p1);
291 //Update column P1
292 for (int i = p1 + 1; i < D.length; i++) {
293 L[lIndex(i, p1)] = L[lIndex(i, p2)];
294 }
295 D[p1] = L[lIndex(p1, p2)];
296 //Update column P2
297 for (int i = p2 + 1; i < D.length; i++) {
298 L[lIndex(i, p2)] = colP1[i];
299 }
300 D[p2] = colP1[p2];
301 } else {
302 //Does nothing when p1 <= p2
303 }
304 }
305
306 /**Get the row of the covariance matrix at the given position (count from 0).
307 * @param L lower part of the covariance matrix
308 * @param D diagonal part of the covariance matrix
309 * @param pos wanted position
310 * @return array of double being the row of covariance matrix at given position
311 */
312 private double[] getRow(final double[] L, final double[] D, final int pos) {
313 final double[] row = new double[D.length];
314 for (int j = 0; j < pos; j++) {
315 row[j] = L[lIndex(pos, j)];
316 }
317 row[pos] = D[pos];
318 for (int j = pos + 1; j < D.length; j++) {
319 row[j] = L[lIndex(j, pos)];
320 }
321 return row;
322 }
323
324 /**Getter for column the greatest at the right side.
325 * @param L double array being the lower triangular matrix
326 * @param D double array being the diagonal matrix
327 * @param row double array being the row of the matrix at given position
328 * @param pmin position at which we get the column
329 * @param pmax other positions
330 * @return array of double
331 */
332 private double[] getColPmax(final double[] L, final double[] D, final double[] row, final int pmin, final int pmax) {
333 final double[] col = new double[D.length];
334 col[pmin] = row[pmax];
335 for (int i = pmin + 1; i < pmax; i++) {
336 col[i] = row[i];
337 }
338 col[pmax] = D[pmax];
339 for (int i = pmax + 1; i < D.length; i++) {
340 col[i] = L[lIndex(i, pmax)];
341 }
342 return col;
343 }
344
345 /** Perform the following operation: W(k,1:k-1) = W(k,1:k-1)/W(k,k) where W is the covariance matrix.
346 * @param mat lower triangular part of the covaraince matrix
347 * @param diag double: value of the covaraicne matrix at psoition (k;k)
348 * @param k integer needed
349 */
350 private void divide(final double[] mat, final double diag, final int k) {
351 for (int j = 0; j < k; j++) {
352 mat[lIndex(k, j)] = mat[lIndex(k, j)] / diag;
353 }
354 }
355
356 /**Inverse the position of 2 element of the array in parameters.
357 * @param mat array on which change should be done
358 * @param p1 position of the first element to change
359 * @param p2 position of the second element to change
360 * @return
361 */
362 private void exchangeIntP1WithIntP2(final int[] mat, final int p1, final int p2) {
363 final int[] Z = mat.clone();
364 mat[p1] = Z[p2];
365 mat[p2] = Z[p1];
366 }
367
368 /** On the array of double mat exchange the element at position p1 with the one at position p2.
369 * @param mat array on which the exchange is performed
370 * @param p1 first position of exchange (0 is the first element)
371 * @param p2 second position of exchange (0 is the first element)
372 */
373 private void exchangeDoubleP1WithDoubleP2(final double[] mat, final int p1, final int p2) {
374 final double[] a = mat.clone();
375 mat[p1] = a[p2];
376 mat[p2] = a[p1];
377 }
378
379 /** Return the symbol of parameter a.
380 * @param a the double for which we want the want the symbol
381 * @return -1.0 if a is lower than or equal to 0 or 1.0 if a is greater than 0
382 */
383 protected double sign(final double a) {
384 return (a <= 0.0) ? -1.0 : ((a > 0.0) ? 1.0 : a);
385 }
386
387 }