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.linear.MatrixUtils;
20 import org.hipparchus.linear.QRDecomposer;
21 import org.hipparchus.linear.RealMatrix;
22 import org.hipparchus.special.Erf;
23 import org.hipparchus.util.FastMath;;
24
25 /** Bootstrapping engine for ILS problem solving.
26 * This method is base on the following paper: <a
27 * href="https://www.researchgate.net/publication/225773077_Success_probability_of_integer_GPS_ambiguity_rounding_and_bootstrapping">
28 * Success probability of integer GPs ambiguity rounding and bootstrapping</a> by P. J. G. Teunissen 1998 and
29 * <a
30 * href="https://repository.tudelft.nl/islandora/object/uuid%3A1a5b8a6e-c9d6-45e3-8e75-48db6d27a523">
31 * Influence of ambiguity precision on the success rate of GNSS integer ambiguity bootstrapping</a> by
32 * P. J. G. Teunissen 2006.
33 * <p>
34 * This method is really faster for integer ambiguity resolution than LAMBDA or MLAMBDA method but its success rate
35 * is really smaller. The method extends LambdaMethod as it uses LDL' factorization and reduction methods from LAMBDA method.
36 * The method is really different from LAMBDA as the solution found is not a least-square solution. It is a solution which asses
37 * a probability of success of the solution found. The probability increase with the does with LDL' factorization and reduction
38 * methods.
39 * </p> <p>
40 * If one want to use this method for integer ambiguity resolution, one just need to construct IntegerBootstrapping
41 * only with a double which is the minimal probability of success one wants.
42 * Then from it, one can call the solveILS method.
43 * @author David Soulard
44 * @since 10.2
45 */
46
47 public class IntegerBootstrapping extends LambdaMethod {
48
49 /** Minimum probability for acceptance. */
50 private double minProb;
51
52 /** Upperbound of the probability. */
53 private boolean boostrapUse;
54
55 /** Integer ambiguity solution from bootstrap method. */
56 private long[] a_B;
57
58 /** Probability of success of the solution found.*/
59 private double p_aB;
60
61 /** Constructor for the bootstrapping ambiguity estimator.
62 * @param prob minimum probability acceptance for the bootstrap
63 */
64 public IntegerBootstrapping(final double prob) {
65 this.minProb = prob;
66 }
67
68 /**
69 * Compute the solution by the bootstrap method from equation (13) in
70 * P.J.G. Teunissen November 2006. The solution is a solution in the
71 * distorted space from LdL' and Z transformation.
72 */
73 @Override
74 protected void discreteSearch() {
75 //If the probability success upper bound is greater than the min probability, bootstrapUse = true, false otherwise
76 this.boostrapUse = upperBoundProbabilitySuccess() > this.minProb;
77 //Getter of the diagonal part and lower part of the covariance matrix
78 final double[] diag = getDiagReference();
79 final double[] low = getLowReference();
80 final int n = diag.length;
81 if (boostrapUse) {
82 final RealMatrix I = MatrixUtils.createRealIdentityMatrix(n);
83 a_B = new long[n];
84 final RealMatrix L = getSymmetricMatrixFromLowPart(low);
85 final RealMatrix invL_I = new QRDecomposer(1.0e-10).
86 decompose(L).getInverse().subtract(I);
87 final double[] decorrelated = getDecorrelatedReference();
88 a_B[0] = (long) FastMath.rint(decorrelated[0]);
89 for (int i = 1; i < a_B.length; i++) {
90 double a_b = 0;
91 for (int j = 0; j < i; j++) {
92 a_b += invL_I.getEntry(i, j) * a_B[j];
93 }
94 a_B[i] = (long) FastMath.rint(decorrelated[i] + a_b);
95 }
96 // Compute the probability of correct integer estimation
97 p_aB = bootstrappedSuccessRate(diag, a_B);
98 if (p_aB > minProb) {
99 this.boostrapUse = true;
100 } else {
101 this.boostrapUse = false;
102 }
103 }
104 }
105
106 /** {@inheritDoc} */
107 @Override
108 protected IntegerLeastSquareSolution[] recoverAmbiguities() {
109 if (boostrapUse) {
110 // get references to the diagonal and lower triangular parts
111 final double[] diag = getDiagReference();
112 final int n = diag.length;
113 final int[] zInverseTransformation = getZInverseTransformationReference();
114 final long[] a = new long[n];
115 for (int i = 0; i < n; ++i) {
116 // compute a = Z⁻ᵀ.s
117 long ai = 0;
118 int l = zIndex(0, i);
119 for (int j = 0; j < n; ++j) {
120 ai += zInverseTransformation[l] * a_B[j];
121 l += n;
122 }
123 a[i] = ai;
124 }
125 a_B = a;
126 final IntegerLeastSquareSolution sol = new IntegerLeastSquareSolution(a_B, p_aB);
127 return new IntegerLeastSquareSolution[] {sol};
128 }
129 else {
130 // Return an empty array
131 return new IntegerLeastSquareSolution[0];
132 }
133 }
134
135 /** Return the matrix symmetric from its low triangular part (1 on the diagonal).
136
137 * @param l lower triangular part of the matrix
138 * @return matrix
139 */
140 private RealMatrix getSymmetricMatrixFromLowPart(final double[] l) {
141 final double[] diag = getDiagReference();
142 final int n = diag.length;
143 final RealMatrix L = MatrixUtils.createRealMatrix(n, n);
144 for (int i = 0; i < n; i++) {
145 for (int j = 0; j < i; j++) {
146 L.setEntry(i, j, l[lIndex(i, j)]);
147 }
148 L.setEntry(i, i, 1.0);
149 }
150 return L;
151 }
152
153 /**Compute the success rate of a bootstraped ILS problem solution.
154 * @param D diagonal of the covaraicne matrix
155 * @param aB bootstrapped solution
156 * @return probability of success
157 */
158 private double bootstrappedSuccessRate(final double[] D, final long[] aB) {
159 double p = 2.0 * phi(1 / (2.0 * D[0]) - 1.0);
160 for (int i = 1; i < D.length; i++) {
161 p = p * (2.0 * phi(1.0 / (2.0 * D[i])) - 1.0);
162 }
163 return p;
164 }
165
166 /** Compute at point x the the value of phi function.
167 * Where phi = 1/2 *(1 + Erf(x/sqrt(2))
168 * @param x value at which we compute phi function
169 * @return value of phi(x)
170 */
171 private double phi(final double x) {
172 return 0.5 * (1.0 + Erf.erf(x / FastMath.sqrt(2.0)));
173 }
174
175 /** Compute the upper bound probability of the ILS problem.
176 * @return upper bound probability of the ILS problem
177 */
178 private double upperBoundProbabilitySuccess() {
179 //covariance matrix determinant
180 double det = 1;
181 final double[] diag = getDiagReference();
182 final int n = diag.length;
183 for (double d: diag) {
184 det *= d;
185 }
186 //ADOP: Ambiguity Dilution of Precision
187 final double adop = FastMath.pow(det, 1.0 / ((double) 2.0 * n));
188 return FastMath.pow(2.0 * phi(1.0 / (2.0 * adop)) - 1.0, n);
189 }
190 }