1 /* Copyright 2002-2021 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 /** Compute the LᵀDL factorization with symmetric pivoting decomposition of Q
37 * (symmetric definite positive matrix) with a minimum symmetric pivoting: Q = ZᵀLᵀDLZ.
38 */
39 @Override
40 protected void ltdlDecomposition() {
41 final double[] diag = getDiagReference();
42 final double[] low = getLowReference();
43 final int[] Z = getZInverseTransformationReference();
44 final double[] aNew = getDecorrelatedReference();
45 for (int i = 0; i < this.getSize(); i++) {
46 Z[zIndex(i, i)] = 0;
47 }
48 final int n = diag.length;
49 final int[] perm = new int[n];
50 for (int i = 0; i < n; i++) {
51 perm[i] = i;
52 }
53 for (int k = n - 1; k >= 0; k--) {
54 final int q = posMin(diag, k);
55 exchangeIntP1WithIntP2(perm, k, q);
56 permRowThenCol(low, diag, k, q);
57 if (k > 0) {
58 final double Wkk = diag[k];
59 divide(low, Wkk, k);
60 set(low, diag, k);
61 }
62 exchangeDoubleP1WithDoubleP2(aNew, k, q);
63 }
64 for (int j = 0; j < n; j++) {
65 Z[zIndex(j, perm[j])] = 1;
66 }
67 }
68
69 /** Perform MLAMBDA reduction.
70 */
71 @Override
72 protected void reduction() {
73 final double[] diag = getDiagReference();
74 final double[] low = getLowReference();
75 final int n = diag.length;
76 int k = getSize() - 2;
77 while ( k > -1) {
78 final int kp1 = k + 1;
79 double tmp = low[lIndex(kp1, k)];
80 final double mu = FastMath.rint(tmp);
81 if (Math.abs(mu) > 1e-9) {
82 tmp -= mu;
83 }
84 final double delta = diag[k] + tmp * tmp * diag[kp1];
85 if (delta < diag[kp1]) {
86 integerGaussTransformation(kp1, k);
87 if (mu > 0) {
88 for (int i = k + 2; i < n; i++) {
89 integerGaussTransformation(i, k);
90 }
91 }
92 permutation(k, delta);
93 if (k < n - 2) {
94 k++;
95 }
96 } else {
97 k--;
98 }
99 }
100 }
101
102 /** {@inheritDoc} */
103 @Override
104 protected void discreteSearch() {
105 //initialization
106 final int n = getSize();
107 final int maxSolutions = getMaxSolution();
108 final double[] diag = getDiagReference();
109 final double[] decorrelated = getDecorrelatedReference();
110 final double[] low = getLowReference();
111 final long[] z = new long[n];
112 final double[] zb = new double[n];
113 final double[] y = new double[n];
114 final int[] path = new int[n];
115
116 for (int i = 0; i < n; i++) {
117 path[i] = n - 1;
118 }
119
120 final double[] step = new double[n];
121 final double[] dist = new double[n + 1];
122 final double[] lS = new double[(n * (n - 1)) / 2];
123 final double[] dS = new double[n];
124 double maxDist = Double.POSITIVE_INFINITY;
125 int count = 0;
126 boolean endSearch = false;
127 int ulevel = 0;
128
129 // Determine which level to move to after z(0) is chosen at level 1.
130 final int k0;
131 if (maxSolutions == 1) {
132 k0 = 1;
133 }
134 else {
135 k0 = 0;
136 }
137
138 // Initialization at level n
139 zb[n - 1] = decorrelated.clone()[n - 1];
140 z[n - 1] = (long) FastMath.rint(zb[n - 1]);
141 y[n - 1] = zb[n - 1] - z[n - 1];
142 step[n - 1] = sign(y[n - 1]);
143 int k = n - 1;
144 while (!endSearch) {
145 for (int i = ulevel; i <= k - 1; i++) {
146 path[i] = k;
147 }
148 for (int j = ulevel - 1; j >= 0; j--) {
149 if (path[j] < k) {
150 path[j] = k;
151 } else {
152 break;
153 }
154 }
155 double newDist = dist[k] + y[k] * y[k] / diag[k];
156 while (newDist < maxDist) {
157 // move to level k-1
158 if (k != 0) {
159 k--;
160 dist[k] = newDist;
161 for (int j = path[k]; j > k; j--) {
162 if (j - 1 == k) {
163 //Update diagonal
164 dS[k] = lS[lIndex(j, k)] - y[j] * low[lIndex(j, k)];
165 } else {
166 //Update low triangular part
167 lS[lIndex(j - 1, k)] = lS[lIndex(j, k)] - y[j] * low[lIndex(j, k)];
168 }
169 }
170
171 zb[k] = decorrelated[k] + dS[k];
172 z[k] = (long) FastMath.rint(zb[k]);
173 y[k] = zb[k] - z[k];
174 step[k] = sign(y[k]);
175 } else {
176 //Save the value of one optimum z
177 if (count < (maxSolutions - 1)) {
178 addSolution(z, newDist);
179 count++;
180 //Save the value of one optimum z
181 } else if (count == (maxSolutions - 1)) {
182 addSolution(z, newDist);
183 maxDist = getMaxDistance();
184 count++;
185 //Replace the new solution with the one with the greatest distance
186 } else {
187 removeSolution();
188 addSolution(z, newDist);
189 maxDist = getMaxDistance();
190 }
191 k = k0;
192 z[k] = z[k] + (long) step[k];
193 y[k] = zb[k] - z[k];
194 step[k] = -step[k] - sign(step[k]);
195 }
196 newDist = dist[k] + y[k] * y[k] / diag[k];
197 }
198 ulevel = k;
199 //exit or move to level k+1
200 while (newDist >= maxDist) {
201 if (k == n - 1) {
202 endSearch = true;
203 break;
204 }
205 //move to level k+1
206 k++;
207 //next integer
208 z[k] = z[k] + (long) step[k];
209 y[k] = zb[k] - z[k];
210 step[k] = -step[k] - sign(step[k]);
211 newDist = dist[k] + y[k] * y[k] / diag[k];
212 }
213 }
214 }
215
216 /** {@inheritDoc} */
217 @Override
218 protected void inverseDecomposition() {
219 // nothing for M-LAMBDA method
220 }
221
222 /** Return the position of the smallest element in the diagonal of a matrix given in parameter.
223 * @param k the position of the smallest diagonal element
224 * @param D an array of double being the diagonal of the covariance matrix
225 * @return the position of the smallest element of mat.
226 */
227 private int posMin(final double[] D, final int k) {
228 int q = 0;
229 double value = D[0];
230 for (int i = 1; i <= k; i++) {
231 if (value > D[i]) {
232 value = D[i];
233 q = i;
234 }
235 }
236 return q;
237 }
238
239 /** Perform the following operation on the matrix W in parameters.
240 * 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);
241 * @param L array of double being a lower triangular part of the covariance matrix
242 * @param D array of double being the diagonal of the covariance matrix
243 * @param p integer at which the computation is done
244 */
245 private void set(final double[] L, final double[] D, final int p) {
246 final double d = D[p];
247 final double[] row = new double[p];
248 for (int i = 0; i < p; i++) {
249 row[i] = L[lIndex(p, i)];
250 }
251 for (int i = 0; i < p; i++) {
252 for (int j = 0; j < i; j++) {
253 L[lIndex(i, j)] = L[lIndex(i, j)] - row[i] * row[j] * d;
254 }
255 D[i] = D[i] - row[i] * row[i] * d;
256 }
257 }
258
259 /** Perform a permutation between two row and then two column of the covariance matrix.
260 * @param L array of double being the lower triangular part of the covariance matrix
261 * @param D array of double being the diagonal of the covariance matrix
262 * @param p1 integer: position of the permutation
263 * @param p2 integer: position of the permutation
264 */
265 private void permRowThenCol(final double[] L, final double[] D, final int p1, final int p2) {
266 final double[] rowP1 = getRow(L, D, p1);
267 final double[] rowP2 = getRow(L, D, p2);
268 if (p1 > p2) {
269 //Update row P2
270 for (int j = 0; j < p2; j++) {
271 L[lIndex(p2, j)] = rowP1[j];
272 }
273 D[p2] = rowP1[p2];
274 //Update row p1
275 for (int j = 0; j < p1; j++) {
276 L[lIndex(p1, j)] = rowP2[j];
277 }
278 D[p1] = rowP2[p1];
279 final double[] colP1 = getColPmax(L, D, rowP1, p2, p1);
280 //Update column P1
281 for (int i = p1 + 1; i < D.length; i++) {
282 L[lIndex(i, p1)] = L[lIndex(i, p2)];
283 }
284 D[p1] = L[lIndex(p1, p2)];
285 //Update column P2
286 for (int i = p2 + 1; i < D.length; i++) {
287 L[lIndex(i, p2)] = colP1[i];
288 }
289 D[p2] = colP1[p2];
290 } else {
291 //Does nothing when p1 <= p2
292 }
293 }
294
295 /**Get the row of the covariance matrix at the given position (count from 0).
296 * @param L lower part of the covariance matrix
297 * @param D diagonal part of the covariance matrix
298 * @param pos wanted position
299 * @return array of double being the row of covariance matrix at given position
300 */
301 private double[] getRow(final double[] L, final double[] D, final int pos) {
302 final double[] row = new double[D.length];
303 for (int j = 0; j < pos; j++) {
304 row[j] = L[lIndex(pos, j)];
305 }
306 row[pos] = D[pos];
307 for (int j = pos + 1; j < D.length; j++) {
308 row[j] = L[lIndex(j, pos)];
309 }
310 return row;
311 }
312
313 /**Getter for column the greatest at the right side.
314 * @param L double array being the lower triangular matrix
315 * @param D double array being the diagonal matrix
316 * @param row double array being the row of the matrix at given position
317 * @param pmin position at which we get the column
318 * @param pmax other positions
319 * @return array of double
320 */
321 private double[] getColPmax(final double[] L, final double[] D, final double[] row, final int pmin, final int pmax) {
322 final double[] col = new double[D.length];
323 col[pmin] = row[pmax];
324 for (int i = pmin + 1; i < pmax; i++) {
325 col[i] = row[i];
326 }
327 col[pmax] = D[pmax];
328 for (int i = pmax + 1; i < D.length; i++) {
329 col[i] = L[lIndex(i, pmax)];
330 }
331 return col;
332 }
333
334 /** Perform the following operation: W(k,1:k-1) = W(k,1:k-1)/W(k,k) where W is the covariance matrix.
335 * @param mat lower triangular part of the covaraince matrix
336 * @param diag double: value of the covaraicne matrix at psoition (k;k)
337 * @param k integer needed
338 */
339 private void divide(final double[] mat, final double diag, final int k) {
340 for (int j = 0; j < k; j++) {
341 mat[lIndex(k, j)] = mat[lIndex(k, j)] / diag;
342 }
343 }
344
345 /**Inverse the position of 2 element of the array in parameters.
346 * @param mat array on which change should be done
347 * @param p1 position of the first element to change
348 * @param p2 position of the second element to change
349 * @return
350 */
351 private void exchangeIntP1WithIntP2(final int[] mat, final int p1, final int p2) {
352 final int[] Z = mat.clone();
353 mat[p1] = Z[p2];
354 mat[p2] = Z[p1];
355 }
356
357 /** On the array of double mat exchange the element at position p1 with the one at position p2.
358 * @param mat array on which the exchange is performed
359 * @param p1 first position of exchange (0 is the first element)
360 * @param p2 second position of exchange (0 is the first element)
361 */
362 private void exchangeDoubleP1WithDoubleP2(final double[] mat, final int p1, final int p2) {
363 final double[] a = mat.clone();
364 mat[p1] = a[p2];
365 mat[p2] = a[p1];
366 }
367
368 /** Return the symbol of parameter a.
369 * @param a the double for which we want the want the symbol
370 * @return -1.0 if a is lower than or equal to 0 or 1.0 if a is greater than 0
371 */
372 protected double sign(final double a) {
373 return (a <= 0.0) ? -1.0 : ((a > 0.0) ? 1.0 : a);
374 }
375
376 }