AmbiguitySolver.java

  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;

  19. import java.util.ArrayList;
  20. import java.util.Collections;
  21. import java.util.List;
  22. import java.util.stream.Collectors;

  24. import org.hipparchus.linear.MatrixUtils;
  25. import org.hipparchus.linear.QRDecomposer;
  26. import org.hipparchus.linear.RealMatrix;
  27. import org.hipparchus.linear.RealVector;
  28. import org.hipparchus.util.FastMath;
  29. import org.orekit.errors.OrekitIllegalArgumentException;
  30. import org.orekit.errors.OrekitMessages;
  31. import org.orekit.utils.ParameterDriver;
  32. import org.orekit.utils.TimeSpanMap.Span;

  34. /** Class for solving integer ambiguity problems.
  35.  * @see LambdaMethod
  36.  * @author Luc Maisonobe
  37.  * @since 10.0
  38.  */
  39. public class AmbiguitySolver {

  41.     /** Drivers for ambiguity drivers. */
  42.     private final List<ParameterDriver> ambiguityDrivers;

  44.     /** Solver for the underlying Integer Least Square problem. */
  45.     private final IntegerLeastSquareSolver solver;

  47.     /** Acceptance test to use. */
  48.     private final AmbiguityAcceptance acceptance;

  50.     /** Simple constructor.
  51.      * @param ambiguityDrivers drivers for ambiguity parameters
  52.      * @param solver solver for the underlying Integer Least Square problem
  53.      * @param acceptance acceptance test to use
  54.      * @see LambdaMethod
  55.      */
  56.     public AmbiguitySolver(final List<ParameterDriver> ambiguityDrivers,
  57.                            final IntegerLeastSquareSolver solver,
  58.                            final AmbiguityAcceptance acceptance) {
  59.         this.ambiguityDrivers = ambiguityDrivers;
  60.         this.solver           = solver;
  61.         this.acceptance       = acceptance;
  62.     }

  64.     /** Get all the ambiguity parameters drivers.
  65.      * @return all ambiguity parameters drivers
  66.      */
  67.     public List<ParameterDriver> getAllAmbiguityDrivers() {
  68.         return Collections.unmodifiableList(ambiguityDrivers);
  69.     }

  71.     /** Get the ambiguity parameters drivers that have not been fixed yet.
  72.      * @return ambiguity parameters drivers that have not been fixed yet
  73.      */
  74.     protected List<ParameterDriver> getFreeAmbiguityDrivers() {
  75.         return ambiguityDrivers.
  76.                         stream().
  77.                         filter(d -> {
  78.                             if (d.isSelected()) {
  79.                                 // in order to make the code generic and compatible with pDriver having
  80.                                 // 1 or several values driven getValue is called with a "random date"
  81.                                 // it should be OK as we take the near number
  82.                                 final double near   = FastMath.rint(d.getValue());
  83.                                 final double gapMin = near - d.getMinValue();
  84.                                 final double gapMax = d.getMaxValue() - near;
  85.                                 return FastMath.max(FastMath.abs(gapMin), FastMath.abs(gapMax)) > 1.0e-15;
  86.                             } else {
  87.                                 return false;
  88.                             }
  89.                         }).
  90.                         collect(Collectors.toList());
  91.     }

  93.     /** Get ambiguity indirection array for ambiguity parameters drivers that have not been fixed yet.
  94.      * @param startIndex start index for measurements parameters in global covariance matrix
  95.      * @param measurementsParametersDrivers measurements parameters drivers in global covariance matrix order
  96.      * @return indirection array between full covariance matrix and ambiguity covariance matrix
  97.      */
  98.     protected int[] getFreeAmbiguityIndirection(final int startIndex,
  99.                                                 final List<ParameterDriver> measurementsParametersDrivers) {

  101.         // set up indirection array
  102.         final List<ParameterDriver> freeDrivers = getFreeAmbiguityDrivers();
  103.         final List<String> measurementsPDriversNames = new ArrayList<>();
  104.         int totalValuesToEstimate = 0;
  105.         for (ParameterDriver driver : freeDrivers) {
  106.             totalValuesToEstimate += driver.getNbOfValues();
  107.         }
  108.         for (ParameterDriver measDriver : measurementsParametersDrivers) {
  109.             for (Span<String> spanMeasurementsParametersDrivers = measDriver.getNamesSpanMap().getFirstSpan();
  110.                     spanMeasurementsParametersDrivers != null; spanMeasurementsParametersDrivers = spanMeasurementsParametersDrivers.next()) {
  111.                 measurementsPDriversNames.add(spanMeasurementsParametersDrivers.getData());
  112.             }

  114.         }

  116.         final int[] indirection = new int[totalValuesToEstimate];
  117.         int nb = 0;
  118.         for (ParameterDriver freeDriver : freeDrivers) {

  120.             for (Span<String> spanFreeDriver = freeDriver.getNamesSpanMap().getFirstSpan(); spanFreeDriver != null; spanFreeDriver = spanFreeDriver.next()) {
  121.                 indirection[nb] = -1;

  123.                 for (int k = 0; k < measurementsPDriversNames.size(); ++k) {
  124.                     if (spanFreeDriver.getData().equals(measurementsPDriversNames.get(k))) {
  125.                         indirection[nb] = startIndex + k;
  126.                         break;
  127.                     }
  128.                 }

  130.                 if (indirection[nb] < 0) {
  131.                     // the parameter was not found
  132.                     final StringBuilder builder = new StringBuilder();
  133.                     for (final String driverName : measurementsPDriversNames) {
  134.                         if (builder.length() > 0) {
  135.                             builder.append(", ");
  136.                         }
  137.                         builder.append(driverName);
  138.                     }
  139.                     throw new OrekitIllegalArgumentException(OrekitMessages.UNSUPPORTED_PARAMETER_NAME,
  140.                             spanFreeDriver.getData(), builder.toString());
  141.                 }
  142.                 nb++;
  143.             }
  144.         }

  146.         return indirection;

  148.     }

  150.     /** Un-fix an integer ambiguity (typically after a phase cycle slip).
  151.      * @param ambiguityDriver driver for the ambiguity to un-fix
  152.      */
  153.     public void unFixAmbiguity(final ParameterDriver ambiguityDriver) {
  154.         ambiguityDriver.setMinValue(Double.NEGATIVE_INFINITY);
  155.         ambiguityDriver.setMaxValue(Double.POSITIVE_INFINITY);
  156.     }

  158.     /** Fix integer ambiguities.
  159.      * @param startIndex start index for measurements parameters in global covariance matrix
  160.      * @param measurementsParametersDrivers measurements parameters drivers in global covariance matrix order
  161.      * @param covariance global covariance matrix
  162.      * @return list of newly fixed ambiguities (ambiguities already fixed before the call are not counted)
  163.      */
  164.     public List<ParameterDriver> fixIntegerAmbiguities(final int startIndex,
  165.                                                        final List<ParameterDriver> measurementsParametersDrivers,
  166.                                                        final RealMatrix covariance) {

  168.         // set up Integer Least Square problem
  169.         final List<ParameterDriver> ambiguities      = getAllAmbiguityDrivers();

  171.         // construct floatambiguities array
  172.         int nbPDriver = 0;
  173.         for (ParameterDriver pDriver : ambiguities) {
  174.             nbPDriver += pDriver.getNbOfValues();
  175.         }
  176.         final double[]              floatAmbiguities = new double[nbPDriver];
  177.         int floatAmbRank = 0;
  178.         for (ParameterDriver pDriver : ambiguities) {
  179.             for (Span<Double> span = pDriver.getValueSpanMap().getFirstSpan(); span != null; span = span.next()) {
  180.                 floatAmbiguities[floatAmbRank++] = span.getData();
  181.             }
  182.         }

  184.         final int[]                 indirection      = getFreeAmbiguityIndirection(startIndex, measurementsParametersDrivers);
  185.         // solve the ILS problem
  186.         final IntegerLeastSquareSolution[] candidates =
  187.                         solver.solveILS(acceptance.numberOfCandidates(), floatAmbiguities, indirection, covariance);

  189.         // FIXME A cleaner way is:
  190.         //     1°/ Add a getName() method in IntegerLeastSquareSolver interface
  191.         //     2°/ Add static name attribute to IntegerBootstrapping, LAMBDA and ModifiedLAMBDA classes
  192.         if (solver instanceof IntegerBootstrapping && candidates.length == 0) {
  193.             return Collections.emptyList();
  194.         }

  196.         // check number of candidates
  197.         if (candidates.length < acceptance.numberOfCandidates()) {
  198.             return Collections.emptyList();
  199.         }

  201.         // check acceptance
  202.         final IntegerLeastSquareSolution bestCandidate = acceptance.accept(candidates);
  203.         if (bestCandidate == null) {
  204.             return Collections.emptyList();
  205.         }

  207.         // fix the ambiguities
  208.         final long[] fixedAmbiguities = bestCandidate.getSolution();
  209.         final List<ParameterDriver> fixedDrivers = new ArrayList<>(indirection.length);
  210.         int nb = 0;
  211.         for (int i = 0; i < measurementsParametersDrivers.size(); ++i) {
  212.             final ParameterDriver driver = measurementsParametersDrivers.get(indirection[nb] - startIndex);
  213.             driver.setMinValue(fixedAmbiguities[i]);
  214.             driver.setMaxValue(fixedAmbiguities[i]);
  215.             fixedDrivers.add(driver);
  216.             nb += driver.getNbOfValues();
  217.         }

  219.         // Update the others parameter drivers accordingly to the fixed integer ambiguity
  220.         // Covariance matrix between integer ambiguity and the other parameter driver
  221.         final RealMatrix Qab = getCovMatrix(covariance, indirection);

  223.         final RealVector X = new QRDecomposer(1.0e-10).decompose(getAmbiguityMatrix(covariance, indirection)).solve(MatrixUtils.createRealVector(floatAmbiguities).
  224.                                                                                                            subtract(MatrixUtils.createRealVector(toDoubleArray(fixedAmbiguities.length, fixedAmbiguities))));
  225.         final RealVector Y =  Qab.preMultiply(X);

  227.         int entry = 0;
  228.         for (int i = startIndex + 1; i < covariance.getColumnDimension(); i++) {
  229.             if (!belongTo(indirection, i)) {
  230.                 final ParameterDriver driver = measurementsParametersDrivers.get(i - startIndex);
  231.                 for (Span<Double> span = driver.getValueSpanMap().getFirstSpan(); span != null; span = span.next()) {

  233.                     driver.setValue(driver.getValue(span.getStart()) - Y.getEntry(entry++ - startIndex), span.getStart());
  234.                 }
  235.             }
  236.         }

  238.         return fixedDrivers;

  240.     }

  242.    /** Get the covariance matrix between the integer ambiguities and the other parameter driver.
  243.     * @param cov global covariance matrix
  244.     * @param indirection array of the position of integer ambiguity parameter driver
  245.     * @return covariance matrix.
  246.     */
  247.     private RealMatrix getCovMatrix(final RealMatrix cov, final int[] indirection) {
  248.         final RealMatrix Qab = MatrixUtils.createRealMatrix(indirection.length, cov.getColumnDimension());
  249.         int index = 0;
  250.         int iter  = 0;
  251.         while (iter < indirection.length) {
  252.             // Loop on column dimension
  253.             for (int j = 0; j < cov.getColumnDimension(); j++) {
  254.                 if (!belongTo(indirection, j)) {
  255.                     Qab.setEntry(index, 0, cov.getEntry(index, 0));
  256.                 }
  257.             }
  258.             index++;
  259.             iter++;
  260.         }
  261.         return Qab;
  262.     }

  264.      /** Return the matrix of the ambiguity from the global covariance matrix.
  265.       * @param cov global covariance matrix
  266.       * @param indirection array of the position of the ambiguity within the global covariance matrix
  267.       * @return matrix of ambiguities covariance
  268.       */
  269.     private RealMatrix getAmbiguityMatrix(final RealMatrix cov, final int[] indirection) {
  270.         final RealMatrix Qa = MatrixUtils.createRealMatrix(indirection.length, indirection.length);
  271.         for (int i = 0; i < indirection.length; i++) {
  272.             Qa.setEntry(i, i, cov.getEntry(indirection[i], indirection[i]));
  273.             for (int j = 0; j < i; j++) {
  274.                 Qa.setEntry(i, j, cov.getEntry(indirection[i], indirection[j]));
  275.                 Qa.setEntry(j, i, cov.getEntry(indirection[i], indirection[j]));
  276.             }
  277.         }
  278.         return Qa;
  279.     }

  281.     /** Compute whether or not the integer pos belongs to the indirection array.
  282.      * @param indirection array of the position of ambiguities within the global covariance matrix
  283.      * @param pos integer for which we want to know if it belong to the indirection array.
  284.      * @return true if it belongs.
  285.      */
  286.     private boolean belongTo(final int[] indirection, final int pos) {
  287.         for (int j : indirection) {
  288.             if (pos == j) {
  289.                 return true;
  290.             }
  291.         }
  292.         return false;
  293.     }

  295.     /** Transform an array of long to an array of double.
  296.      * @param size size of the destination array
  297.      * @param longArray source array
  298.      * @return the destination array
  299.      */
  300.     private double[] toDoubleArray(final int size, final long[] longArray) {
  301.         // Initialize double array
  302.         final double[] doubleArray = new double[size];
  303.         // Copy the elements
  304.         for (int index = 0; index < size; index++) {
  305.             doubleArray[index] = longArray[index];
  306.         }
  307.         return doubleArray;
  308.     }

  310. }
Created with JaCoCo 0.8.12.202403310830