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 java.util.Comparator;
20 import java.util.SortedSet;
21 import java.util.TreeSet;
22
23 import org.hipparchus.linear.RealMatrix;
24 import org.hipparchus.util.FastMath;
25
26 /** Base class for decorrelation/reduction engine for LAMBDA type methods.
27 * <p>
28 * This class is based on both the 1996 paper <a href="https://www.researchgate.net/publication/2790708_The_LAMBDA_method_for_integer_ambiguity_estimation_implementation_aspects">
29 * The LAMBDA method for integer ambiguity estimation: implementation aspects</a> by
30 * Paul de Jonge and Christian Tiberius and on the 2005 paper <a
31 * href="https://www.researchgate.net/publication/225518977_MLAMBDA_a_modified_LAMBDA_method_for_integer_least-squares_estimation">
32 * A modified LAMBDA method for integer least-squares estimation</a> by X.-W Chang, X. Yang
33 * and T. Zhou, Journal of Geodesy 79(9):552-565, DOI: 10.1007/s00190-005-0004-x
34 * </p>
35 * @author Luc Maisonobe
36 * @since 10.0
37 */
38 public abstract class AbstractLambdaMethod implements IntegerLeastSquareSolver {
39
40 /** Number of ambiguities. */
41 private int n;
42
43 /** Decorrelated ambiguities. */
44 private double[] decorrelated;
45
46 /** Lower triangular matrix with unit diagonal, in row order (unit diagonal not stored). */
47 private double[] low;
48
49 /** Diagonal matrix. */
50 private double[] diag;
51
52 /** Z⁻¹ transformation matrix, in row order. */
53 private int[] zInverseTransformation;
54
55 /** Maximum number of solutions seeked. */
56 private int maxSolutions;
57
58 /** Placeholder for solutions found. */
59 private SortedSet<IntegerLeastSquareSolution> solutions;
60
61 /** Comparator for integer least square solutions. */
62 private Comparator<IntegerLeastSquareSolution> comparator;
63
64 /** Constructor.
65 * <p>
66 * By default a {@link IntegerLeastSquareComparator} is used
67 * to compare integer least square solutions
68 * </p>
69 */
70 protected AbstractLambdaMethod() {
71 this.comparator = new IntegerLeastSquareComparator();
72 }
73
74 /** {@inheritDoc} */
75 @Override
76 public IntegerLeastSquareSolution[] solveILS(final int nbSol, final double[] floatAmbiguities,
77 final int[] indirection, final RealMatrix covariance) {
78
79 // initialize the ILS problem search
80 initializeProblem(floatAmbiguities, indirection, covariance, nbSol);
81
82 // perform initial Lᵀ.D.L = Q decomposition of covariance
83 ltdlDecomposition();
84
85 // perform decorrelation/reduction of covariances
86 reduction();
87
88 // transform the Lᵀ.D.L = Q decomposition of covariance into
89 // the L⁻¹.D⁻¹.L⁻ᵀ = Q⁻¹ decomposition of the inverse of covariance.
90 inverseDecomposition();
91
92 // perform discrete search of Integer Least Square problem
93 discreteSearch();
94
95 return recoverAmbiguities();
96
97 }
98
99 /** Set a custom comparator for integer least square solutions comparison.
100 * <p>
101 * Calling this method overrides any comparator that could have been set
102 * beforehand. It also overrides the default {@link IntegerLeastSquareComparator}.
103 * </p>
104 * @param newCompartor new comparator to use
105 * @since 11.0
106 */
107 public void setComparator(final Comparator<IntegerLeastSquareSolution> newCompartor) {
108 this.comparator = newCompartor;
109 }
110
111 /** Initialize ILS problem.
112 * @param floatAmbiguities float estimates of ambiguities
113 * @param indirection indirection array to extract ambiguity covariances from global covariance matrix
114 * @param globalCovariance global covariance matrix (includes ambiguities among other parameters)
115 * @param nbSol number of solutions to search for
116 */
117 private void initializeProblem(final double[] floatAmbiguities, final int[] indirection,
118 final RealMatrix globalCovariance, final int nbSol) {
119
120 this.n = floatAmbiguities.length;
121 this.decorrelated = floatAmbiguities.clone();
122 this.low = new double[(n * (n - 1)) / 2];
123 this.diag = new double[n];
124 this.zInverseTransformation = new int[n * n];
125 this.maxSolutions = nbSol;
126 this.solutions = new TreeSet<>(comparator);
127
128 // initialize decomposition matrices
129 for (int i = 0; i < n; ++i) {
130 for (int j = 0; j < i; ++j) {
131 low[lIndex(i, j)] = globalCovariance.getEntry(indirection[i], indirection[j]);
132 }
133 diag[i] = globalCovariance.getEntry(indirection[i], indirection[i]);
134 zInverseTransformation[zIndex(i, i)] = 1;
135 }
136
137 }
138
139 /** Get a reference to the diagonal matrix of the decomposition.
140 * <p>
141 * BEWARE: the returned value is a reference to an internal array,
142 * it is <em>only</em> intended for subclasses use (hence the
143 * method is protected and not public).
144 * </p>
145 * @return reference to the diagonal matrix of the decomposition
146 */
147 protected double[] getDiagReference() {
148 return diag;
149 }
150
151 /** Get a reference to the lower triangular matrix of the decomposition.
152 * <p>
153 * BEWARE: the returned value is a reference to an internal array,
154 * it is <em>only</em> intended for subclasses use (hence the
155 * method is protected and not public).
156 * </p>
157 * @return reference to the lower triangular matrix of the decomposition
158 */
159 protected double[] getLowReference() {
160 return low;
161 }
162
163 /** Get the reference decorrelated ambiguities.
164 * @return reference to the decorrelated ambiguities.
165 **/
166 protected double[] getDecorrelatedReference() {
167 return decorrelated;
168 }
169
170 /** Get the maximum number of solutions seeked.
171 * @return the maximum number of solutions seeked
172 */
173 protected int getMaxSolution() {
174 return maxSolutions;
175 }
176
177 /** Add a new solution.
178 * @param fixed solution array
179 * @param squaredNorm squared distance to the corresponding float solution
180 */
181 protected void addSolution(final long[] fixed, final double squaredNorm) {
182 solutions.add(new IntegerLeastSquareSolution(fixed, squaredNorm));
183 }
184
185 /** Remove spurious solution.
186 */
187 protected void removeSolution() {
188 solutions.remove(solutions.last());
189 }
190
191 /** Get the number of solutions found.
192 * @return the number of solutions found
193 */
194 protected int getSolutionsSize() {
195 return solutions.size();
196 }
197
198 /**Get the maximum of distance among the solutions found.
199 * getting last of solutions as they are sorted in SortesSet
200 * @return greatest distance of the solutions
201 * @since 10.2
202 */
203 protected double getMaxDistance() {
204 return solutions.last().getSquaredDistance();
205 }
206
207 /** Get a reference to the Z inverse transformation matrix.
208 * <p>
209 * BEWARE: the returned value is a reference to an internal array,
210 * it is <em>only</em> intended for subclasses use (hence the
211 * method is protected and not public).
212 * BEWARE: for the MODIFIED LAMBDA METHOD, the returned matrix Z is such that
213 * Q = Z'L'DLZ where Q is the covariance matrix and ' refers to the transposition operation
214 * @return array of integer corresponding to Z matrix
215 * @since 10.2
216 */
217 protected int[] getZInverseTransformationReference() {
218 return zInverseTransformation;
219 }
220
221 /** Get the size of the problem. In the ILS problem, the integer returned
222 * is the size of the covariance matrix.
223 * @return the size of the ILS problem
224 * @since 10.2
225 */
226 protected int getSize() {
227 return n;
228 }
229
230 /** Perform Lᵀ.D.L = Q decomposition of the covariance matrix.
231 */
232 protected abstract void ltdlDecomposition();
233
234 /** Perform LAMBDA reduction.
235 */
236 protected abstract void reduction();
237
238 /** Find the best solutions to the Integer Least Square problem.
239 */
240 protected abstract void discreteSearch();
241
242 /** Inverse the decomposition.
243 * <p>
244 * This method transforms the Lᵀ.D.L = Q decomposition of covariance into
245 * the L⁻¹.D⁻¹.L⁻ᵀ = Q⁻¹ decomposition of the inverse of covariance.
246 * </p>
247 */
248 protected abstract void inverseDecomposition();
249
250 /** Perform one integer Gauss transformation.
251 * <p>
252 * This method corresponds to algorithm 2.1 in X.-W Chang, X. Yang and T. Zhou paper.
253 * </p>
254 * @param row row index (counting from 0)
255 * @param col column index (counting from 0)
256 */
257 protected void integerGaussTransformation(final int row, final int col) {
258 final int mu = (int) FastMath.rint(low[lIndex(row, col)]);
259 if (mu != 0) {
260
261 // update low triangular matrix (post-multiplying L by Zᵢⱼ = I - μ eᵢ eⱼᵀ)
262 low[lIndex(row, col)] -= mu;
263 for (int i = row + 1; i < n; ++i) {
264 low[lIndex(i, col)] -= mu * low[lIndex(i, row)];
265 }
266
267 // update Z⁻¹ transformation matrix
268 for (int i = 0; i < n; ++i) {
269 // pre-multiplying Z⁻¹ by Zᵢⱼ⁻¹ = I + μ eᵢ eⱼᵀ
270 zInverseTransformation[zIndex(row, i)] += mu * zInverseTransformation[zIndex(col, i)];
271 }
272
273 // update decorrelated ambiguities estimates (pre-multiplying a by Zᵢⱼᵀ = I - μ eⱼ eᵢᵀ)
274 decorrelated[col] -= mu * decorrelated[row];
275
276 }
277 }
278
279 /** Perform one symmetric permutation involving rows/columns {@code k0} and {@code k0+1}.
280 * <p>
281 * This method corresponds to algorithm 2.2 in X.-W Chang, X. Yang and T. Zhou paper.
282 * </p>
283 * @param k0 diagonal index (counting from 0)
284 * @param delta new value for diag[k0+1]
285 */
286 protected void permutation(final int k0, final double delta) {
287
288 final int k1 = k0 + 1;
289 final int k2 = k0 + 2;
290 final int indexk1k0 = lIndex(k1, k0);
291 final double lk1k0 = low[indexk1k0];
292 final double eta = diag[k0] / delta;
293 final double lambda = diag[k1] * lk1k0 / delta;
294
295 // update diagonal
296 diag[k0] = eta * diag[k1];
297 diag[k1] = delta;
298
299 // update low triangular matrix
300 for (int j = 0; j < k0; ++j) {
301 final int indexk0j = lIndex(k0, j);
302 final int indexk1j = lIndex(k1, j);
303 final double lk0j = low[indexk0j];
304 final double lk1j = low[indexk1j];
305 low[indexk0j] = lk1j - lk1k0 * lk0j;
306 low[indexk1j] = lambda * lk1j + eta * lk0j;
307 }
308 low[indexk1k0] = lambda;
309 for (int i = k2; i < n; ++i) {
310 final int indexik0 = lIndex(i, k0);
311 final int indexik1 = indexik0 + 1;
312 final double tmp = low[indexik0];
313 low[indexik0] = low[indexik1];
314 low[indexik1] = tmp;
315 }
316
317 // update Z⁻¹ transformation matrix
318 for (int i = 0; i < n; ++i) {
319
320 final int indexk0i = zIndex(k0, i);
321 final int indexk1i = indexk0i + n;
322 final int tmp2 = zInverseTransformation[indexk0i];
323 zInverseTransformation[indexk0i] = zInverseTransformation[indexk1i];
324 zInverseTransformation[indexk1i] = tmp2;
325
326 }
327
328 // update decorrelated ambiguities
329 final double tmp = decorrelated[k0];
330 decorrelated[k0] = decorrelated[k1];
331 decorrelated[k1] = tmp;
332
333 }
334
335 /** Recover ambiguities prior to the Z-transformation.
336 * @return recovered ambiguities
337 */
338 protected IntegerLeastSquareSolution[] recoverAmbiguities() {
339
340 final IntegerLeastSquareSolution[] recovered = new IntegerLeastSquareSolution[solutions.size()];
341
342 int k = 0;
343 final long[] a = new long[n];
344 for (final IntegerLeastSquareSolution solution : solutions) {
345 final long[] s = solution.getSolution();
346 for (int i = 0; i < n; ++i) {
347 // compute a = Z⁻ᵀ.s
348 long ai = 0;
349 int l = zIndex(0, i);
350 for (int j = 0; j < n; ++j) {
351 ai += zInverseTransformation[l] * s[j];
352 l += n;
353 }
354 a[i] = ai;
355 }
356 recovered[k++] = new IntegerLeastSquareSolution(a, solution.getSquaredDistance());
357 }
358
359 return recovered;
360
361 }
362
363 /** Get the index of an entry in the lower triangular matrix.
364 * @param row row index (counting from 0)
365 * @param col column index (counting from 0)
366 * @return index in the single dimension array
367 */
368 protected int lIndex(final int row, final int col) {
369 return (row * (row - 1)) / 2 + col;
370 }
371
372 /** Get the index of an entry in the Z transformation matrix.
373 * @param row row index (counting from 0)
374 * @param col column index (counting from 0)
375 * @return index in the single dimension array
376 */
377 protected int zIndex(final int row, final int col) {
378 return row * n + col;
379 }
380
381 }