AbstractLambdaMethod.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.Comparator;
  20. import java.util.SortedSet;
  21. import java.util.TreeSet;

  23. import org.hipparchus.linear.RealMatrix;
  24. import org.hipparchus.util.FastMath;

  26. /** Base class for decorrelation/reduction engine for LAMBDA type methods.
  27.  * <p>
  28.  * This class is based on both the 1996 paper <a href="https://www.researchgate.net/publication/2790708_The_LAMBDA_method_for_integer_ambiguity_estimation_implementation_aspects">
  29.  * The LAMBDA method for integer ambiguity estimation: implementation aspects</a> by
  30.  * Paul de Jonge and Christian Tiberius and on the 2005 paper <a
  31.  * href="https://www.researchgate.net/publication/225518977_MLAMBDA_a_modified_LAMBDA_method_for_integer_least-squares_estimation">
  32.  * A modified LAMBDA method for integer least-squares estimation</a> by X.-W Chang, X. Yang
  33.  * and T. Zhou, Journal of Geodesy 79(9):552-565, DOI: 10.1007/s00190-005-0004-x
  34.  * </p>
  35.  * @author Luc Maisonobe
  36.  * @since 10.0
  37.  */
  38. public abstract class AbstractLambdaMethod implements IntegerLeastSquareSolver {

  40.     /** Number of ambiguities. */
  41.     private int n;

  43.     /** Decorrelated ambiguities. */
  44.     private double[] decorrelated;

  46.     /** Lower triangular matrix with unit diagonal, in row order (unit diagonal not stored). */
  47.     private double[] low;

  49.     /** Diagonal matrix. */
  50.     private double[] diag;

  52.     /** Z⁻¹ transformation matrix, in row order. */
  53.     private int[] zInverseTransformation;

  55.     /** Maximum number of solutions seeked. */
  56.     private int maxSolutions;

  58.     /** Placeholder for solutions found. */
  59.     private SortedSet<IntegerLeastSquareSolution> solutions;

  61.     /** Comparator for integer least square solutions. */
  62.     private Comparator<IntegerLeastSquareSolution> comparator;

  64.     /** Constructor.
  65.      * <p>
  66.      * By default a {@link IntegerLeastSquareComparator} is used
  67.      * to compare integer least square solutions
  68.      * </p>
  69.      */
  70.     protected AbstractLambdaMethod() {
  71.         this.comparator = new IntegerLeastSquareComparator();
  72.     }

  74.     /** {@inheritDoc} */
  75.     @Override
  76.     public IntegerLeastSquareSolution[] solveILS(final int nbSol, final double[] floatAmbiguities,
  77.                                                  final int[] indirection, final RealMatrix covariance) {

  79.         // initialize the ILS problem search
  80.         initializeProblem(floatAmbiguities, indirection, covariance, nbSol);

  82.         // perform initial Lᵀ.D.L = Q decomposition of covariance
  83.         ltdlDecomposition();

  85.         // perform decorrelation/reduction of covariances
  86.         reduction();

  88.         // transform the Lᵀ.D.L = Q decomposition of covariance into
  89.         // the L⁻¹.D⁻¹.L⁻ᵀ = Q⁻¹ decomposition of the inverse of covariance.
  90.         inverseDecomposition();

  92.         // perform discrete search of Integer Least Square problem
  93.         discreteSearch();

  95.         return recoverAmbiguities();

  97.     }

  99.     /** Set a custom comparator for integer least square solutions comparison.
  100.      * <p>
  101.      * Calling this method overrides any comparator that could have been set
  102.      * beforehand. It also overrides the default {@link IntegerLeastSquareComparator}.
  103.      * </p>
  104.      * @param newCompartor new comparator to use
  105.      * @since 11.0
  106.      */
  107.     public void setComparator(final Comparator<IntegerLeastSquareSolution> newCompartor) {
  108.         this.comparator = newCompartor;
  109.     }

  111.     /** Initialize ILS problem.
  112.      * @param floatAmbiguities float estimates of ambiguities
  113.      * @param indirection indirection array to extract ambiguity covariances from global covariance matrix
  114.      * @param globalCovariance global covariance matrix (includes ambiguities among other parameters)
  115.      * @param nbSol number of solutions to search for
  116.      */
  117.     private void initializeProblem(final double[] floatAmbiguities, final int[] indirection,
  118.                                    final RealMatrix globalCovariance, final int nbSol) {

  120.         this.n                      = floatAmbiguities.length;
  121.         this.decorrelated           = floatAmbiguities.clone();
  122.         this.low                    = new double[(n * (n - 1)) / 2];
  123.         this.diag                   = new double[n];
  124.         this.zInverseTransformation = new int[n * n];
  125.         this.maxSolutions           = nbSol;
  126.         this.solutions              = new TreeSet<>(comparator);

  128.         // initialize decomposition matrices
  129.         for (int i = 0; i < n; ++i) {
  130.             for (int j = 0; j < i; ++j) {
  131.                 low[lIndex(i, j)] = globalCovariance.getEntry(indirection[i], indirection[j]);
  132.             }
  133.             diag[i] = globalCovariance.getEntry(indirection[i], indirection[i]);
  134.             zInverseTransformation[zIndex(i, i)] = 1;
  135.         }

  137.     }

  139.     /** Get a reference to the diagonal matrix of the decomposition.
  140.      * <p>
  141.      * BEWARE: the returned value is a reference to an internal array,
  142.      * it is <em>only</em> intended for subclasses use (hence the
  143.      * method is protected and not public).
  144.      * </p>
  145.      * @return reference to the diagonal matrix of the decomposition
  146.      */
  147.     protected double[] getDiagReference() {
  148.         return diag;
  149.     }

  151.     /** Get a reference to the lower triangular matrix of the decomposition.
  152.      * <p>
  153.      * BEWARE: the returned value is a reference to an internal array,
  154.      * it is <em>only</em> intended for subclasses use (hence the
  155.      * method is protected and not public).
  156.      * </p>
  157.      * @return reference to the lower triangular matrix of the decomposition
  158.      */
  159.     protected double[] getLowReference() {
  160.         return low;
  161.     }

  163.     /** Get the reference decorrelated ambiguities.
  164.      * @return reference to the decorrelated ambiguities.
  165.      **/
  166.     protected double[] getDecorrelatedReference() {
  167.         return decorrelated;
  168.     }

  170.     /** Get the maximum number of solutions seeked.
  171.      * @return the maximum number of solutions seeked
  172.      */
  173.     protected int getMaxSolution() {
  174.         return maxSolutions;
  175.     }

  177.     /** Add a new solution.
  178.      * @param fixed solution array
  179.      * @param squaredNorm squared distance to the corresponding float solution
  180.      */
  181.     protected void addSolution(final long[] fixed, final double squaredNorm) {
  182.         solutions.add(new IntegerLeastSquareSolution(fixed, squaredNorm));
  183.     }

  185.     /** Remove spurious solution.
  186.      */
  187.     protected void removeSolution() {
  188.         solutions.remove(solutions.last());
  189.     }

  191.     /** Get the number of solutions found.
  192.      * @return the number of solutions found
  193.      */
  194.     protected int getSolutionsSize() {
  195.         return solutions.size();
  196.     }

  198.     /**Get the maximum of distance among the solutions found.
  199.      * getting last of solutions as they are sorted in SortesSet
  200.      * @return greatest distance of the solutions
  201.      * @since 10.2
  202.      */
  203.     protected double getMaxDistance() {
  204.         return solutions.last().getSquaredDistance();
  205.     }

  207.     /** Get a reference to the Z  inverse transformation matrix.
  208.      * <p>
  209.      * BEWARE: the returned value is a reference to an internal array,
  210.      * it is <em>only</em> intended for subclasses use (hence the
  211.      * method is protected and not public).
  212.      * BEWARE: for the MODIFIED LAMBDA METHOD, the returned matrix Z is such that
  213.      * Q = Z'L'DLZ where Q is the covariance matrix and ' refers to the transposition operation
  214.      * @return array of integer corresponding to Z matrix
  215.      * @since 10.2
  216.      */
  217.     protected int[] getZInverseTransformationReference() {
  218.         return zInverseTransformation;
  219.     }

  221.     /** Get the size of the problem. In the ILS problem, the integer returned
  222.      * is the size of the covariance matrix.
  223.      * @return the size of the ILS problem
  224.      * @since 10.2
  225.      */
  226.     protected int getSize() {
  227.         return n;
  228.     }

  230.     /** Perform Lᵀ.D.L = Q decomposition of the covariance matrix.
  231.      */
  232.     protected abstract void ltdlDecomposition();

  234.     /** Perform LAMBDA reduction.
  235.      */
  236.     protected abstract void reduction();

  238.     /** Find the best solutions to the Integer Least Square problem.
  239.      */
  240.     protected abstract void discreteSearch();

  242.     /** Inverse the decomposition.
  243.      * <p>
  244.      * This method transforms the Lᵀ.D.L = Q decomposition of covariance into
  245.      * the L⁻¹.D⁻¹.L⁻ᵀ = Q⁻¹ decomposition of the inverse of covariance.
  246.      * </p>
  247.      */
  248.     protected abstract void inverseDecomposition();

  250.     /** Perform one integer Gauss transformation.
  251.      * <p>
  252.      * This method corresponds to algorithm 2.1 in X.-W Chang, X. Yang and T. Zhou paper.
  253.      * </p>
  254.      * @param row row index (counting from 0)
  255.      * @param col column index (counting from 0)
  256.      */
  257.     protected void integerGaussTransformation(final int row, final int col) {
  258.         final int mu = (int) FastMath.rint(low[lIndex(row, col)]);
  259.         if (mu != 0) {

  261.             // update low triangular matrix (post-multiplying L by Zᵢⱼ = I - μ eᵢ eⱼᵀ)
  262.             low[lIndex(row, col)] -= mu;
  263.             for (int i = row + 1; i < n; ++i) {
  264.                 low[lIndex(i, col)] -= mu * low[lIndex(i, row)];
  265.             }

  267.             // update Z⁻¹ transformation matrix
  268.             for (int i = 0; i < n; ++i) {
  269.                 // pre-multiplying Z⁻¹ by Zᵢⱼ⁻¹ = I + μ eᵢ eⱼᵀ
  270.                 zInverseTransformation[zIndex(row, i)] += mu * zInverseTransformation[zIndex(col, i)];
  271.             }

  273.             // update decorrelated ambiguities estimates (pre-multiplying a by  Zᵢⱼᵀ = I - μ eⱼ eᵢᵀ)
  274.             decorrelated[col] -= mu * decorrelated[row];

  276.         }
  277.     }

  279.     /** Perform one symmetric permutation involving rows/columns {@code k0} and {@code k0+1}.
  280.      * <p>
  281.      * This method corresponds to algorithm 2.2 in X.-W Chang, X. Yang and T. Zhou paper.
  282.      * </p>
  283.      * @param k0 diagonal index (counting from 0)
  284.      * @param delta new value for diag[k0+1]
  285.      */
  286.     protected void permutation(final int k0, final double delta) {

  288.         final int    k1        = k0 + 1;
  289.         final int    k2        = k0 + 2;
  290.         final int    indexk1k0 = lIndex(k1, k0);
  291.         final double lk1k0     = low[indexk1k0];
  292.         final double eta       = diag[k0] / delta;
  293.         final double lambda    = diag[k1] * lk1k0 / delta;

  295.         // update diagonal
  296.         diag[k0] = eta * diag[k1];
  297.         diag[k1] = delta;

  299.         // update low triangular matrix
  300.         for (int j = 0; j < k0; ++j) {
  301.             final int indexk0j = lIndex(k0, j);
  302.             final int indexk1j = lIndex(k1, j);
  303.             final double lk0j  = low[indexk0j];
  304.             final double lk1j  = low[indexk1j];
  305.             low[indexk0j]      = lk1j          - lk1k0 * lk0j;
  306.             low[indexk1j]      = lambda * lk1j + eta   * lk0j;
  307.         }
  308.         low[indexk1k0] = lambda;
  309.         for (int i = k2; i < n; ++i) {
  310.             final int indexik0 = lIndex(i, k0);
  311.             final int indexik1 = indexik0 + 1;
  312.             final double tmp   = low[indexik0];
  313.             low[indexik0]      = low[indexik1];
  314.             low[indexik1]      = tmp;
  315.         }

  317.         // update Z⁻¹ transformation matrix
  318.         for (int i = 0; i < n; ++i) {

  320.             final int indexk0i               = zIndex(k0, i);
  321.             final int indexk1i               = indexk0i + n;
  322.             final int tmp2                   = zInverseTransformation[indexk0i];
  323.             zInverseTransformation[indexk0i] = zInverseTransformation[indexk1i];
  324.             zInverseTransformation[indexk1i] = tmp2;

  326.         }

  328.         // update decorrelated ambiguities
  329.         final double tmp = decorrelated[k0];
  330.         decorrelated[k0] = decorrelated[k1];
  331.         decorrelated[k1] = tmp;

  333.     }

  335.     /** Recover ambiguities prior to the Z-transformation.
  336.      * @return recovered ambiguities
  337.      */
  338.     protected IntegerLeastSquareSolution[] recoverAmbiguities() {

  340.         final IntegerLeastSquareSolution[] recovered = new IntegerLeastSquareSolution[solutions.size()];

  342.         int k = 0;
  343.         final long[] a = new long[n];
  344.         for (final IntegerLeastSquareSolution solution : solutions) {
  345.             final long[] s = solution.getSolution();
  346.             for (int i = 0; i < n; ++i) {
  347.                 // compute a = Z⁻ᵀ.s
  348.                 long ai = 0;
  349.                 int l = zIndex(0, i);
  350.                 for (int j = 0; j < n; ++j) {
  351.                     ai += zInverseTransformation[l] * s[j];
  352.                     l  += n;
  353.                 }
  354.                 a[i] = ai;
  355.             }
  356.             recovered[k++] = new IntegerLeastSquareSolution(a, solution.getSquaredDistance());
  357.         }

  359.         return recovered;

  361.     }

  363.     /** Get the index of an entry in the lower triangular matrix.
  364.      * @param row row index (counting from 0)
  365.      * @param col column index (counting from 0)
  366.      * @return index in the single dimension array
  367.      */
  368.     protected int lIndex(final int row, final int col) {
  369.         return (row * (row - 1)) / 2 + col;
  370.     }

  372.     /** Get the index of an entry in the Z transformation matrix.
  373.      * @param row row index (counting from 0)
  374.      * @param col column index (counting from 0)
  375.      * @return index in the single dimension array
  376.      */
  377.     protected int zIndex(final int row, final int col) {
  378.         return row * n + col;
  379.     }

  381. }
Created with JaCoCo 0.8.12.202403310830