AmbiguitySolver.java

  1. /* Copyright 2002-2020 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. import java.util.ArrayList;
  19. import java.util.Collections;
  20. import java.util.List;
  21. import java.util.stream.Collectors;

  22. import org.hipparchus.linear.RealMatrix;
  23. import org.hipparchus.util.FastMath;
  24. import org.orekit.errors.OrekitIllegalArgumentException;
  25. import org.orekit.errors.OrekitMessages;
  26. import org.orekit.utils.ParameterDriver;

  27. /** Class for solving integer ambiguity problems.
  28.  * @see LambdaMethod
  29.  * @author Luc Maisonobe
  30.  * @since 10.0
  31.  */
  32. public class AmbiguitySolver {

  33.     /** Drivers for ambiguity drivers. */
  34.     private final List<ParameterDriver> ambiguityDrivers;

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

  37.     /** Acceptance test to use. */
  38.     private final AmbiguityAcceptance acceptance;

  39.     /** Simple constructor.
  40.      * @param ambiguityDrivers drivers for ambiguity parameters
  41.      * @param solver solver for the underlying Integer Least Square problem
  42.      * @param acceptance acceptance test to use
  43.      * @see LambdaMethod
  44.      */
  45.     public AmbiguitySolver(final List<ParameterDriver> ambiguityDrivers,
  46.                            final IntegerLeastSquareSolver solver,
  47.                            final AmbiguityAcceptance acceptance) {
  48.         this.ambiguityDrivers = ambiguityDrivers;
  49.         this.solver           = solver;
  50.         this.acceptance       = acceptance;
  51.     }

  52.     /** Get all the ambiguity parameters drivers.
  53.      * @return all ambiguity parameters drivers
  54.      */
  55.     public List<ParameterDriver> getAllAmbiguityDrivers() {
  56.         return Collections.unmodifiableList(ambiguityDrivers);
  57.     }

  58.     /** Get the ambiguity parameters drivers that have not been fixed yet.
  59.      * @return ambiguity parameters drivers that have not been fixed yet
  60.      */
  61.     protected List<ParameterDriver> getFreeAmbiguityDrivers() {
  62.         return ambiguityDrivers.
  63.                         stream().
  64.                         filter(d -> {
  65.                             if (d.isSelected()) {
  66.                                 final double near   = FastMath.rint(d.getValue());
  67.                                 final double gapMin = near - d.getMinValue();
  68.                                 final double gapMax = d.getMaxValue() - near;
  69.                                 return FastMath.max(FastMath.abs(gapMin), FastMath.abs(gapMax)) > 1.0e-15;
  70.                             } else {
  71.                                 return false;
  72.                             }
  73.                         }).
  74.                         collect(Collectors.toList());
  75.     }

  76.     /** Get ambiguity indirection array for ambiguity parameters drivers that have not been fixed yet.
  77.      * @param startIndex start index for measurements parameters in global covariance matrix
  78.      * @param measurementsParametersDrivers measurements parameters drivers in global covariance matrix order
  79.      * @return indirection array between full covariance matrix and ambiguity covariance matrix
  80.      */
  81.     protected int[] getFreeAmbiguityIndirection(final int startIndex,
  82.                                                 final List<ParameterDriver> measurementsParametersDrivers) {

  83.         // set up indirection array
  84.         final List<ParameterDriver> freeDrivers = getFreeAmbiguityDrivers();
  85.         final int n = freeDrivers.size();
  86.         final int[] indirection = new int[n];
  87.         for (int i = 0; i < n; ++i) {
  88.             indirection[i] = -1;
  89.             final String name = freeDrivers.get(i).getName();
  90.             for (int k = 0; k < measurementsParametersDrivers.size(); ++k) {
  91.                 if (name.equals(measurementsParametersDrivers.get(k).getName())) {
  92.                     indirection[i] = startIndex + k;
  93.                     break;
  94.                 }
  95.             }
  96.             if (indirection[i] < 0) {
  97.                 // the parameter was not found
  98.                 final StringBuilder builder = new StringBuilder();
  99.                 for (final ParameterDriver driver : measurementsParametersDrivers) {
  100.                     if (builder.length() > 0) {
  101.                         builder.append(", ");
  102.                     }
  103.                     builder.append(driver.getName());
  104.                 }
  105.                 throw new OrekitIllegalArgumentException(OrekitMessages.UNSUPPORTED_PARAMETER_NAME,
  106.                                                          name, builder.toString());
  107.             }
  108.         }

  109.         return indirection;

  110.     }

  111.     /** Un-fix an integer ambiguity (typically after a phase cycle slip).
  112.      * @param ambiguityDriver driver for the ambiguity to un-fix
  113.      */
  114.     public void unFixAmbiguity(final ParameterDriver ambiguityDriver) {
  115.         ambiguityDriver.setMinValue(Double.NEGATIVE_INFINITY);
  116.         ambiguityDriver.setMaxValue(Double.POSITIVE_INFINITY);
  117.     }

  118.     /** Fix integer ambiguities.
  119.      * @param startIndex start index for measurements parameters in global covariance matrix
  120.      * @param measurementsParametersDrivers measurements parameters drivers in global covariance matrix order
  121.      * @param covariance global covariance matrix
  122.      * @return list of newly fixed ambiguities (ambiguities already fixed before the call are not counted)
  123.      */
  124.     public List<ParameterDriver> fixIntegerAmbiguities(final int startIndex,
  125.                                                        final List<ParameterDriver> measurementsParametersDrivers,
  126.                                                        final RealMatrix covariance) {

  127.         // set up Integer Least Square problem
  128.         final List<ParameterDriver> ambiguities      = getAllAmbiguityDrivers();
  129.         final double[]              floatAmbiguities = ambiguities.stream().mapToDouble(d -> d.getValue()).toArray();
  130.         final int[]                 indirection      = getFreeAmbiguityIndirection(startIndex, measurementsParametersDrivers);

  131.         // solve the ILS problem
  132.         final IntegerLeastSquareSolution[] candidates =
  133.                         solver.solveILS(acceptance.numberOfCandidates(), floatAmbiguities, indirection, covariance);
  134.         if (candidates.length < acceptance.numberOfCandidates()) {
  135.             return Collections.emptyList();
  136.         }

  137.         // check acceptance
  138.         final IntegerLeastSquareSolution bestCandidate = acceptance.accept(candidates);
  139.         if (bestCandidate == null) {
  140.             return Collections.emptyList();
  141.         }

  142.         // fix the ambiguities
  143.         final long[] fixedAmbiguities = bestCandidate.getSolution();
  144.         final List<ParameterDriver> fixedDrivers = new ArrayList<>(indirection.length);
  145.         for (int i = 0; i < indirection.length; ++i) {
  146.             final ParameterDriver driver = measurementsParametersDrivers.get(indirection[i] - startIndex);
  147.             driver.setMinValue(fixedAmbiguities[i]);
  148.             driver.setMaxValue(fixedAmbiguities[i]);
  149.             fixedDrivers.add(driver);
  150.         }

  151.         // TODO
  152.         // update the other float parameter drivers estimates
  153.         // using their correlations with the fixed ambiguities

  154.         return fixedDrivers;

  155.     }

  156. }