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 org.hipparchus.linear.MatrixUtils;
20 import org.hipparchus.linear.RealMatrix;
21 import org.hipparchus.random.RandomGenerator;
22 import org.hipparchus.random.Well19937a;
23 import org.hipparchus.util.FastMath;
24 import org.hipparchus.util.Precision;
25 import org.junit.jupiter.api.Assertions;
26 import org.junit.jupiter.api.Test;
27
28 import java.util.Comparator;
29
30 public class LambdaMethodTest extends AbstractLambdaMethodTest {
31
32 protected AbstractLambdaMethod buildReducer() {
33 return new LambdaMethod();
34 }
35
36 protected RealMatrix buildCovariance(AbstractLambdaMethod reducer) {
37 return getLow(reducer).
38 transposeMultiply(getDiag(reducer)).
39 multiply(getLow(reducer));
40 }
41
42 @Test
43 public void testPermutation() {
44
45 RandomGenerator random = new Well19937a(0xf824c33093974ee5l);
46 for (int k = 0; k < 1000; ++k) {
47
48 final int n = FastMath.max(2, 1 + random.nextInt(20));
49
50 final RealMatrix covariance = createRandomSymmetricPositiveDefiniteMatrix(n, random);
51 final int[] indirection = createRandomIndirectionArray(n, random);
52
53
54 doTestPermutation(random, indirection, covariance);
55
56 }
57 }
58
59 @Test
60 public void testCustomComparator() {
61
62
63 final double[] floatAmbiguities = new double[] {
64 5.450, 3.100, 2.970
65 };
66 final int[] indirection = new int[] { 0, 1, 2 };
67 final RealMatrix covariance = MatrixUtils.createRealMatrix(new double[][] {
68 { 6.290, 5.978, 0.544 },
69 { 5.978, 6.292, 2.340 },
70 { 0.544, 2.340, 6.288 }
71 });
72
73
74 final AbstractLambdaMethod reducer = buildReducer();
75
76
77
78 reducer.setComparator(new Comparator<IntegerLeastSquareSolution>() {
79
80 @Override
81 public int compare(final IntegerLeastSquareSolution o1,
82 final IntegerLeastSquareSolution o2) {
83 return 0;
84 }
85 });
86
87 IntegerLeastSquareSolution[] solutions = reducer.solveILS(1, floatAmbiguities, indirection, covariance);
88
89
90 Assertions.assertEquals(1, solutions.length);
91 Assertions.assertEquals(0.2183310953369383, solutions[0].getSquaredDistance(), 1.0e-15);
92 Assertions.assertEquals(5l, solutions[0].getSolution()[0]);
93 Assertions.assertEquals(3l, solutions[0].getSolution()[1]);
94 Assertions.assertEquals(4l, solutions[0].getSolution()[2]);
95 }
96
97 private void doTestPermutation(final RandomGenerator random,
98 final int[] indirection, final RealMatrix covariance) {
99 final double[] floatAmbiguities = new double[indirection.length];
100 for (int i = 0; i < floatAmbiguities.length; ++i) {
101 floatAmbiguities[i] = 2 * random.nextDouble() - 1.0;
102 }
103 final AbstractLambdaMethod reducer = buildReducer();
104 initializeProblem(reducer, floatAmbiguities, indirection, covariance, 2);
105 reducer.ltdlDecomposition();
106 RealMatrix filteredCovariance = filterCovariance(covariance, indirection);
107 RealMatrix zRef = MatrixUtils.createRealIdentityMatrix(indirection.length);
108 double[] aBase = getDecorrelated(reducer).clone();
109 double[] aRef = aBase.clone();
110 Assertions.assertEquals(0.0,
111 zRef.subtract(getZTransformation(reducer)).getNorm1(),
112 1.0e-15);
113 for (int k = 0; k < 10; ++k) {
114 final Permutation permutation = createRandomPermutation(getLow(reducer),
115 getDiag(reducer),
116 random);
117 reducer.permutation(permutation.i, permutation.delta);
118
119
120 zRef = zRef.multiply(permutation.z);
121 Assertions.assertEquals(0.0,
122 zRef.subtract(getZTransformation(reducer)).getNorm1(),
123 Precision.SAFE_MIN);
124
125
126 RealMatrix rebuilt = getLow(reducer).transposeMultiply(getDiag(reducer)).multiply(getLow(reducer));
127 RealMatrix permuted = zRef.transposeMultiply(filteredCovariance).multiply(zRef);
128 Assertions.assertEquals(0.0,
129 permuted.subtract(rebuilt).getNorm1(),
130 2.7e-12 * filteredCovariance.getNorm1());
131
132
133 final double tmp = aRef[permutation.i];
134 aRef[permutation.i] = aRef[permutation.i + 1];
135 aRef[permutation.i + 1] = tmp;
136 for (int i = 0; i < aRef.length; ++i) {
137 Assertions.assertEquals(aRef[i], getDecorrelated(reducer)[i], 4.0e-14);
138 }
139
140
141 final double[] aRef2 = zRef.transpose().operate(aBase);
142 for (int i = 0; i < aRef2.length; ++i) {
143 Assertions.assertEquals(aRef2[i], getDecorrelated(reducer)[i], 4.0e-14);
144 }
145
146 }
147 }
148
149 @Test
150 public void testIntegerGaussTransformation() {
151
152 RandomGenerator random = new Well19937a(0x08e9e32dcd0f9dbdl);
153 for (int k = 0; k < 1000; ++k) {
154
155 final int n = FastMath.max(2, 1 + random.nextInt(20));
156
157 final RealMatrix covariance = createRandomSymmetricPositiveDefiniteMatrix(n, random);
158 final int[] indirection = createRandomIndirectionArray(n, random);
159
160
161 doTestIntegerGaussTransformation(random, indirection, covariance);
162
163 }
164 }
165
166 private void doTestIntegerGaussTransformation(final RandomGenerator random,
167 final int[] indirection, final RealMatrix covariance) {
168 final int n = indirection.length;
169 final double[] floatAmbiguities = new double[n];
170 for (int i = 0; i < n; ++i) {
171 floatAmbiguities[i] = 2 * random.nextDouble() - 1.0;
172 }
173 final AbstractLambdaMethod reducer = buildReducer();
174 initializeProblem(reducer, floatAmbiguities, indirection, covariance, 2);
175 reducer.ltdlDecomposition();
176 RealMatrix identity = MatrixUtils.createRealIdentityMatrix(n);
177 RealMatrix zRef = identity;
178 RealMatrix lowRef = getLow(reducer);
179 RealMatrix diagRef = getDiag(reducer);
180 double[] aBase = getDecorrelated(reducer).clone();
181 double[] aRef = aBase;
182 Assertions.assertEquals(0.0,
183 zRef.subtract(getZTransformation(reducer)).getNorm1(),
184 1.0e-15);
185 for (int k = 0; k < 10; ++k) {
186 final IntegerGaussTransformation gauss = createRandomIntegerGaussTransformation(getLow(reducer), random);
187 reducer.integerGaussTransformation(gauss.i, gauss.j);
188
189
190 zRef = zRef.multiply(gauss.z);
191 Assertions.assertEquals(0.0,
192 zRef.subtract(getZTransformation(reducer)).getNorm1(),
193 1.5e-15 * zRef.getNorm1());
194
195
196 Assertions.assertEquals(0.0,
197 diagRef.subtract(getDiag(reducer)).getNorm1(),
198 Precision.SAFE_MIN);
199
200
201 lowRef = lowRef.multiply(gauss.z);
202 Assertions.assertEquals(0.0,
203 lowRef.subtract(getLow(reducer)).getNorm1(),
204 Precision.SAFE_MIN);
205 Assertions.assertTrue(getLow(reducer).getEntry(gauss.i, gauss.j) <= 0.5);
206
207
208 aRef = gauss.z.transpose().operate(aRef);
209 for (int i = 0; i < aRef.length; ++i) {
210 Assertions.assertEquals(aRef[i], getDecorrelated(reducer)[i], 4.0e-14);
211 }
212
213
214 final double[] aRef2 = zRef.transpose().operate(aBase);
215 for (int i = 0; i < aRef2.length; ++i) {
216 Assertions.assertEquals(aRef2[i], getDecorrelated(reducer)[i], 2.3e-13);
217 }
218
219 }
220 }
221
222 }