1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation.measurements.gnss;
18
19 import java.lang.reflect.Field;
20 import java.lang.reflect.InvocationTargetException;
21 import java.lang.reflect.Method;
22 import java.util.Arrays;
23
24 import org.hipparchus.linear.DiagonalMatrix;
25 import org.hipparchus.linear.MatrixUtils;
26 import org.hipparchus.linear.QRDecomposer;
27 import org.hipparchus.linear.RealMatrix;
28 import org.hipparchus.random.RandomGenerator;
29 import org.hipparchus.random.Well19937a;
30 import org.hipparchus.util.FastMath;
31 import org.hipparchus.util.MathArrays;
32 import org.hipparchus.util.MathArrays.Position;
33 import org.junit.Assert;
34 import org.junit.Test;
35
36 public abstract class AbstractLambdaMethodTest {
37
38 protected abstract AbstractLambdaMethod buildReducer();
39 protected abstract RealMatrix buildCovariance(AbstractLambdaMethod reducer);
40
41 @Test
42 public void testSimpleFullDecomposition() {
43 final RealMatrix refLow = MatrixUtils.createRealMatrix(new double[][] {
44 { 1.0, 0.0, 0.0, 0.0 },
45 { 2.0, 1.0, 0.0, 0.0 },
46 { 3.0, 4.0, 1.0, 0.0 },
47 { 5.0, 6.0, 7.0, 1.0 }
48 });
49 final RealMatrix refDiag = MatrixUtils.createRealDiagonalMatrix(new double[] {
50 5.0, 7.0, 9.0, 11.0
51 });
52 final RealMatrix covariance = refLow.transposeMultiply(refDiag).multiply(refLow);
53 final int[] indirection = new int[] { 0, 1, 2, 3 };
54 AbstractLambdaMethod reducer = buildReducer();
55 initializeProblem(reducer, new double[indirection.length], indirection, covariance, 2);
56 reducer.ltdlDecomposition();
57 Assert.assertEquals(0.0, refLow.subtract(getLow(reducer)).getNorm1(), 9.9e-13 * refLow.getNorm1());
58 Assert.assertEquals(0.0, refDiag.subtract(getDiag(reducer)).getNorm1(), 6.7e-13 * refDiag.getNorm1());
59 }
60
61 @Test
62 public void testRandomDecomposition() {
63 RandomGenerator random = new Well19937a(0x7aa94f3683fd08c1l);
64 for (int k = 0; k < 1000; ++k) {
65
66 final int n = FastMath.max(2, 1 + random.nextInt(20));
67 final RealMatrix covariance = createRandomSymmetricPositiveDefiniteMatrix(n, random);
68 final int[] indirection = createRandomIndirectionArray(n, random);
69
70 doTestDecomposition(indirection, covariance);
71 }
72 }
73
74 @Test
75 public void testRandomProblems() {
76 RandomGenerator random = new Well19937a(0x1c68f36088a9133al);
77 for (int k = 0; k < 1000; ++k) {
78
79 final int n = FastMath.max(2, 1 + random.nextInt(20));
80 final RealMatrix covariance = createRandomSymmetricPositiveDefiniteMatrix(n, random);
81 final int[] indirection = createRandomIndirectionArray(n, random);
82
83
84 doTestILS(random, indirection, covariance);
85 }
86 }
87
88 @Test
89 public void testJoostenTiberiusFAQ() {
90
91
92 final double[] floatAmbiguities = new double[] {
93 5.450, 3.100, 2.970
94 };
95 final int[] indirection = new int[] { 0, 1, 2 };
96 final RealMatrix covariance = MatrixUtils.createRealMatrix(new double[][] {
97 { 6.290, 5.978, 0.544 },
98 { 5.978, 6.292, 2.340 },
99 { 0.544, 2.340, 6.288 }
100 });
101
102 final AbstractLambdaMethod reducer = buildReducer();
103 IntegerLeastSquareSolution[] solutions = reducer.solveILS(2, floatAmbiguities, indirection, covariance);
104
105
106 Assert.assertEquals(2, solutions.length);
107 Assert.assertEquals(0.2183310953369383, solutions[0].getSquaredDistance(), 1.0e-15);
108 Assert.assertEquals(5l, solutions[0].getSolution()[0]);
109 Assert.assertEquals(3l, solutions[0].getSolution()[1]);
110 Assert.assertEquals(4l, solutions[0].getSolution()[2]);
111 Assert.assertEquals(0.3072725757902666, solutions[1].getSquaredDistance(), 1.0e-12);
112 Assert.assertEquals(6l, solutions[1].getSolution()[0]);
113 Assert.assertEquals(4l, solutions[1].getSolution()[1]);
114 Assert.assertEquals(4l, solutions[1].getSolution()[2]);
115
116 }
117
118 private void doTestDecomposition(final int[] indirection, final RealMatrix covariance) {
119 final AbstractLambdaMethod reducer = buildReducer();
120 initializeProblem(reducer, new double[indirection.length], indirection, covariance, 2);
121 reducer.ltdlDecomposition();
122 final RealMatrix extracted = MatrixUtils.createRealMatrix(indirection.length, indirection.length);
123 for (int i = 0; i < indirection.length; ++i) {
124 for (int j = 0; j < indirection.length; ++j) {
125 extracted.setEntry(i, j, covariance.getEntry(indirection[i], indirection[j]));
126 }
127 }
128 final RealMatrix rebuilt = buildCovariance(reducer);
129 Assert.assertEquals(0.0,
130 rebuilt.subtract(extracted).getNorm1(),
131 2.5e-13 * extracted.getNorm1());
132
133 }
134
135
136 private void doTestILS(final RandomGenerator random,
137 final int[] indirection, final RealMatrix covariance) {
138 final double[] floatAmbiguities = new double[indirection.length];
139 for (int i = 0; i < floatAmbiguities.length; ++i) {
140 floatAmbiguities[i] = 2 * random.nextDouble() - 1.0;
141 }
142 final RealMatrix aHat = MatrixUtils.createColumnRealMatrix(floatAmbiguities);
143 final RealMatrix invCov = new QRDecomposer(1.0e-10).
144 decompose(filterCovariance(covariance, indirection)).
145 getInverse();
146 final AbstractLambdaMethod reducer = buildReducer();
147 final int nbSol = 10;
148 final IntegerLeastSquareSolution[] solutions =
149 reducer.solveILS(nbSol, floatAmbiguities, indirection, covariance);
150
151
152 Assert.assertEquals(nbSol, solutions.length);
153 for (int i = 0; i < nbSol - 1; ++i) {
154 Assert.assertTrue(solutions[i].getSquaredDistance() <= solutions[i + 1].getSquaredDistance());
155 }
156 for (int i = 0; i < nbSol; ++i) {
157 final RealMatrix a = MatrixUtils.createRealMatrix(floatAmbiguities.length, 1);
158 for (int k = 0; k < a.getRowDimension(); ++k) {
159 a.setEntry(k, 0, solutions[i].getSolution()[k]);
160 }
161 final double squaredNorm = a.subtract(aHat).transposeMultiply(invCov).multiply(a.subtract(aHat)).getEntry(0, 0);
162 Assert.assertEquals(squaredNorm, solutions[i].getSquaredDistance(), 6.0e-10 * squaredNorm);
163 }
164
165
166 double min = Double.POSITIVE_INFINITY;
167 for (int i = 0; i < 1000; ++i) {
168 final RealMatrix a = MatrixUtils.createRealMatrix(floatAmbiguities.length, 1);
169 for (int k = 0; k < a.getRowDimension(); ++k) {
170 long close = FastMath.round(floatAmbiguities[k]);
171 a.setEntry(k, 0, close + random.nextInt(11) - 5);
172 }
173 final double squaredNorm = a.subtract(aHat).transposeMultiply(invCov).multiply(a.subtract(aHat)).getEntry(0, 0);
174 min = FastMath.min(min, (squaredNorm - solutions[0].getSquaredDistance()) / solutions[0].getSquaredDistance());
175 }
176 Assert.assertTrue(min > -1.6e-10);
177
178 }
179
180 protected int getN(final AbstractLambdaMethod reducer) {
181 try {
182 final Field nField = AbstractLambdaMethod.class.getDeclaredField("n");
183 nField.setAccessible(true);
184 return ((Integer) nField.get(reducer)).intValue();
185 } catch (NoSuchFieldException | IllegalAccessException e) {
186 Assert.fail(e.getLocalizedMessage());
187 return -1;
188 }
189 }
190
191 protected RealMatrix getLow(final AbstractLambdaMethod reducer) {
192 try {
193 final int n = getN(reducer);
194 final Field lowField = AbstractLambdaMethod.class.getDeclaredField("low");
195 lowField.setAccessible(true);
196 final double[] low = (double[]) lowField.get(reducer);
197 final RealMatrix lowM = MatrixUtils.createRealMatrix(n, n);
198 int k = 0;
199 for (int i = 0; i < n; ++i) {
200 for (int j = 0; j < i; ++j) {
201 lowM.setEntry(i, j, low[k++]);
202 }
203 lowM.setEntry(i, i, 1.0);
204 }
205 return lowM;
206 } catch (NoSuchFieldException | IllegalAccessException e) {
207 Assert.fail(e.getLocalizedMessage());
208 return null;
209 }
210 }
211
212 protected DiagonalMatrix getDiag(final AbstractLambdaMethod reducer) {
213 try {
214 final Field diagField = AbstractLambdaMethod.class.getDeclaredField("diag");
215 diagField.setAccessible(true);
216 return new DiagonalMatrix((double[]) diagField.get(reducer));
217 } catch (NoSuchFieldException | IllegalAccessException e) {
218 Assert.fail(e.getLocalizedMessage());
219 return null;
220 }
221 }
222
223 protected RealMatrix getZTransformation(final AbstractLambdaMethod reducer) {
224 return new QRDecomposer(1.0e-10).decompose(dogetZInverse(reducer)).getInverse();
225 }
226
227 protected RealMatrix getZInverseTransformation(final AbstractLambdaMethod reducer) {
228 return dogetZInverse(reducer);
229 }
230
231 protected RealMatrix dogetZInverse(final AbstractLambdaMethod reducer) {
232 try {
233 final int n = getN(reducer);
234 final Field zField = AbstractLambdaMethod.class.getDeclaredField("zInverseTransformation");
235 zField.setAccessible(true);
236 final int[] z = (int[]) zField.get(reducer);
237 final RealMatrix zM = MatrixUtils.createRealMatrix(n, n);
238 int k = 0;
239 for (int i = 0; i < n; ++i) {
240 for (int j = 0; j < n; ++j) {
241 zM.setEntry(i, j, z[k++]);
242 }
243 }
244 return zM;
245 } catch (NoSuchFieldException | IllegalAccessException e) {
246 Assert.fail(e.getLocalizedMessage());
247 return null;
248 }
249 }
250
251 protected double[] getDecorrelated(final AbstractLambdaMethod reducer) {
252 try {
253 final Field decorrelatedField = AbstractLambdaMethod.class.getDeclaredField("decorrelated");
254 decorrelatedField.setAccessible(true);
255 return (double[]) decorrelatedField.get(reducer);
256 } catch (NoSuchFieldException | IllegalAccessException e) {
257 Assert.fail(e.getLocalizedMessage());
258 return null;
259 }
260 }
261
262 protected RealMatrix createRandomSymmetricPositiveDefiniteMatrix(final int n, final RandomGenerator random) {
263 final RealMatrix matrix = MatrixUtils.createRealMatrix(n, n);
264 for (int i = 0; i < n; ++i) {
265 for (int j = 0; j < n; ++j) {
266 matrix.setEntry(i, j, 20 * random.nextDouble() - 10);
267 }
268 }
269 return matrix.transposeMultiply(matrix);
270 }
271
272 protected int[] createRandomIndirectionArray(final int n, final RandomGenerator random) {
273 final int[] all = new int[n];
274 for (int i = 0; i < all.length; ++i) {
275 all[i] = i;
276 }
277 MathArrays.shuffle(all, 0, Position.TAIL, random);
278 return Arrays.copyOf(all, FastMath.max(2, 1 + random.nextInt(n)));
279 }
280
281 protected IntegerGaussTransformation createRandomIntegerGaussTransformation(final RealMatrix low,
282 final RandomGenerator random) {
283 final int n = low.getRowDimension();
284 int i = random.nextInt(n);
285 int j = i;
286 while (j == i) {
287 j = random.nextInt(n);
288 }
289 if (i < j) {
290 final int tmp = i;
291 i = j;
292 j = tmp;
293 }
294 return new IntegerGaussTransformation(n, i, j, (int) FastMath.rint(low.getEntry(i, j)));
295 }
296
297 protected static class IntegerGaussTransformation {
298 protected final int i;
299 protected final int j;
300 protected final RealMatrix z;
301 IntegerGaussTransformation(int n, int i, int j, int mu) {
302 this.i = i;
303 this.j = j;
304 this.z = MatrixUtils.createRealIdentityMatrix(n);
305 z.setEntry(i, j, -mu);
306 }
307 }
308
309 protected Permutation createRandomPermutation(final RealMatrix low,
310 final RealMatrix diag,
311 final RandomGenerator random) {
312 final int n = low.getRowDimension();
313 final int i = random.nextInt(n - 1);
314 final double dk0 = diag.getEntry(i, i);
315 final double dk1 = diag.getEntry(i + 1, i + 1);
316 final double lk1k0 = low.getEntry(i + 1, i);
317 return new Permutation(n, i, dk0 + lk1k0 * lk1k0 * dk1);
318 }
319
320 protected void initializeProblem(final AbstractLambdaMethod method,
321 final double[] floatAmbiguities, final int[] indirection,
322 final RealMatrix globalCovariance, final int nbSol) {
323 try {
324 final Method initializeMethod = AbstractLambdaMethod.class.getDeclaredMethod("initializeProblem",
325 double[].class,
326 int[].class,
327 RealMatrix.class,
328 Integer.TYPE);
329 initializeMethod.setAccessible(true);
330 initializeMethod.invoke(method, floatAmbiguities, indirection, globalCovariance, nbSol);
331 } catch (NoSuchMethodException | IllegalAccessException |
332 IllegalArgumentException | InvocationTargetException e) {
333 Assert.fail(e.getLocalizedMessage());
334 }
335 }
336
337 protected static class Permutation {
338 protected final int i;
339 protected double delta;
340 protected final RealMatrix z;
341 Permutation(int n, int i, double delta) {
342 this.i = i;
343 this.delta = delta;
344 this.z = MatrixUtils.createRealIdentityMatrix(n);
345 z.setEntry(i, i, 0);
346 z.setEntry(i, i + 1, 1);
347 z.setEntry(i + 1, i, 1);
348 z.setEntry(i + 1, i + 1, 0);
349 }
350 }
351
352 RealMatrix filterCovariance(final RealMatrix covariance, int[] indirection) {
353 RealMatrix filtered = MatrixUtils.createRealMatrix(indirection.length, indirection.length);
354 for (int i = 0; i < indirection.length; ++i) {
355 for (int j = 0; j <= i; ++j) {
356 filtered.setEntry(i, j, covariance.getEntry(indirection[i], indirection[j]));
357 filtered.setEntry(j, i, covariance.getEntry(indirection[i], indirection[j]));
358 }
359 }
360 return filtered;
361 }
362
363 }
364