AbstractLambdaMethod.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;

  19. import java.util.SortedSet;
  20. import java.util.TreeSet;

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

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

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

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

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

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

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

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

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

  60.     /** {@inheritDoc} */
  61.     @Override
  62.     public IntegerLeastSquareSolution[] solveILS(final int nbSol, final double[] floatAmbiguities,
  63.                                                  final int[] indirection, final RealMatrix covariance) {

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

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

  71.         // perform decorrelation/reduction of covariances
  72.         reduction();

  74.         // transform the Lᵀ.D.L = Q decomposition of covariance into
  75.         // the L⁻¹.D⁻¹.L⁻ᵀ = Q⁻¹ decomposition of the inverse of covariance.
  76.         inverseDecomposition();

  78.         // perform discrete search of Integer Least Square problem
  79.         discreteSearch();

  81.         return recoverAmbiguities();

  83.     }

  85.     /** Initialize ILS problem.
  86.      * @param floatAmbiguities float estimates of ambiguities
  87.      * @param indirection indirection array to extract ambiguity covariances from global covariance matrix
  88.      * @param globalCovariance global covariance matrix (includes ambiguities among other parameters)
  89.      * @param nbSol number of solutions to search for
  90.      */
  91.     private void initializeProblem(final double[] floatAmbiguities, final int[] indirection,
  92.                                    final RealMatrix globalCovariance, final int nbSol) {

  94.         this.n                      = floatAmbiguities.length;
  95.         this.decorrelated           = floatAmbiguities.clone();
  96.         this.low                    = new double[(n * (n - 1)) / 2];
  97.         this.diag                   = new double[n];
  98.         this.zInverseTransformation = new int[n * n];
  99.         this.maxSolutions           = nbSol;
  100.         this.solutions              = new TreeSet<>();

  102.         // initialize decomposition matrices
  103.         for (int i = 0; i < n; ++i) {
  104.             for (int j = 0; j < i; ++j) {
  105.                 low[lIndex(i, j)] = globalCovariance.getEntry(indirection[i], indirection[j]);
  106.             }
  107.             diag[i] = globalCovariance.getEntry(indirection[i], indirection[i]);
  108.             zInverseTransformation[zIndex(i, i)] = 1;
  109.         }

  111.     }

  113.     /** Get a reference to the diagonal matrix of the decomposition.
  114.      * <p>
  115.      * BEWARE: the returned value is a reference to an internal array,
  116.      * it is <em>only</em> intended for subclasses use (hence the
  117.      * method is protected and not public).
  118.      * </p>
  119.      * @return reference to the diagonal matrix of the decomposition
  120.      */
  121.     protected double[] getDiagReference() {
  122.         return diag;
  123.     }

  125.     /** Get a reference to the lower triangular matrix of the decomposition.
  126.      * <p>
  127.      * BEWARE: the returned value is a reference to an internal array,
  128.      * it is <em>only</em> intended for subclasses use (hence the
  129.      * method is protected and not public).
  130.      * </p>
  131.      * @return reference to the lower triangular matrix of the decomposition
  132.      */
  133.     protected double[] getLowReference() {
  134.         return low;
  135.     }

  137.     /** Get the reference decorrelated ambiguities.
  138.      * @return reference to the decorrelated ambiguities.
  139.      **/
  140.     protected double[] getDecorrelatedReference() {
  141.         return decorrelated;
  142.     }

  144.     /** Get the maximum number of solutions seeked.
  145.      * @return the maximum number of solutions seeked
  146.      */
  147.     protected int getMaxSolution() {
  148.         return maxSolutions;
  149.     }

  151.     /** Add a new solution.
  152.      * @param fixed solution array
  153.      * @param squaredNorm squared distance to the corresponding float solution
  154.      */
  155.     protected void addSolution(final long[] fixed, final double squaredNorm) {
  156.         solutions.add(new IntegerLeastSquareSolution(fixed, squaredNorm));
  157.     }

  159.     /** Remove spurious solution.
  160.      */
  161.     protected void removeSolution() {
  162.         solutions.remove(solutions.last());
  163.     }

  165.     /** Get the number of solutions found.
  166.      * @return the number of solutions found
  167.      */
  168.     protected int getSolutionsSize() {
  169.         return solutions.size();
  170.     }

  172.     /**Get the maximum of distance among the solutions found.
  173.      * getting last of solutions as they are sorted in SortesSet
  174.      * @return greatest distance of the solutions
  175.      * @since 10.2
  176.      */
  177.     protected double getMaxDistance() {
  178.         return solutions.last().getSquaredDistance();
  179.     }

  181.     /** Get a reference to the Z  inverse transformation matrix.
  182.      * <p>
  183.      * BEWARE: the returned value is a reference to an internal array,
  184.      * it is <em>only</em> intended for subclasses use (hence the
  185.      * method is protected and not public).
  186.      * BEWARE: for the MODIFIED LAMBDA METHOD, the returned matrix Z is such that
  187.      * Q = Z'L'DLZ where Q is the covariance matrix and ' refers to the transposition operation
  188.      * @return array of integer corresponding to Z matrix
  189.      * @since 10.2
  190.      */
  191.     protected int[] getZInverseTransformationReference() {
  192.         return zInverseTransformation;
  193.     }

  195.     /** Get the size of the problem. In the ILS problem, the integer returned
  196.      * is the size of the covariance matrix.
  197.      * @return the size of the ILS problem
  198.      * @since 10.2
  199.      */
  200.     protected int getSize() {
  201.         return n;
  202.     }

  204.     /** Perform Lᵀ.D.L = Q decomposition of the covariance matrix.
  205.      */
  206.     protected abstract void ltdlDecomposition();

  208.     /** Perform LAMBDA reduction.
  209.      */
  210.     protected abstract void reduction();

  212.     /** Find the best solutions to the Integer Least Square problem.
  213.      */
  214.     protected abstract void discreteSearch();

  216.     /** Inverse the decomposition.
  217.      * <p>
  218.      * This method transforms the Lᵀ.D.L = Q decomposition of covariance into
  219.      * the L⁻¹.D⁻¹.L⁻ᵀ = Q⁻¹ decomposition of the inverse of covariance.
  220.      * </p>
  221.      */
  222.     protected abstract void inverseDecomposition();

  224.     /** Perform one integer Gauss transformation.
  225.      * <p>
  226.      * This method corresponds to algorithm 2.1 in X.-W Chang, X. Yang and T. Zhou paper.
  227.      * </p>
  228.      * @param row row index (counting from 0)
  229.      * @param col column index (counting from 0)
  230.      */
  231.     protected void integerGaussTransformation(final int row, final int col) {
  232.         final int mu = (int) FastMath.rint(low[lIndex(row, col)]);
  233.         if (mu != 0) {

  235.             // update low triangular matrix (post-multiplying L by Zᵢⱼ = I - μ eᵢ eⱼᵀ)
  236.             low[lIndex(row, col)] -= mu;
  237.             for (int i = row + 1; i < n; ++i) {
  238.                 low[lIndex(i, col)] -= mu * low[lIndex(i, row)];
  239.             }

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

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

  250.         }
  251.     }

  253.     /** Perform one symmetric permutation involving rows/columns {@code k0} and {@code k0+1}.
  254.      * <p>
  255.      * This method corresponds to algorithm 2.2 in X.-W Chang, X. Yang and T. Zhou paper.
  256.      * </p>
  257.      * @param k0 diagonal index (counting from 0)
  258.      * @param delta new value for diag[k0+1]
  259.      */
  260.     protected void permutation(final int k0, final double delta) {

  262.         final int    k1        = k0 + 1;
  263.         final int    k2        = k0 + 2;
  264.         final int    indexk1k0 = lIndex(k1, k0);
  265.         final double lk1k0     = low[indexk1k0];
  266.         final double eta       = diag[k0] / delta;
  267.         final double lambda    = diag[k1] * lk1k0 / delta;

  269.         // update diagonal
  270.         diag[k0] = eta * diag[k1];
  271.         diag[k1] = delta;

  273.         // update low triangular matrix
  274.         for (int j = 0; j < k0; ++j) {
  275.             final int indexk0j = lIndex(k0, j);
  276.             final int indexk1j = lIndex(k1, j);
  277.             final double lk0j  = low[indexk0j];
  278.             final double lk1j  = low[indexk1j];
  279.             low[indexk0j]      = lk1j          - lk1k0 * lk0j;
  280.             low[indexk1j]      = lambda * lk1j + eta   * lk0j;
  281.         }
  282.         low[indexk1k0] = lambda;
  283.         for (int i = k2; i < n; ++i) {
  284.             final int indexik0 = lIndex(i, k0);
  285.             final int indexik1 = indexik0 + 1;
  286.             final double tmp   = low[indexik0];
  287.             low[indexik0]      = low[indexik1];
  288.             low[indexik1]      = tmp;
  289.         }

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

  294.             final int indexk0i               = zIndex(k0, i);
  295.             final int indexk1i               = indexk0i + n;
  296.             final int tmp2                   = zInverseTransformation[indexk0i];
  297.             zInverseTransformation[indexk0i] = zInverseTransformation[indexk1i];
  298.             zInverseTransformation[indexk1i] = tmp2;

  300.         }

  302.         // update decorrelated ambiguities
  303.         final double tmp = decorrelated[k0];
  304.         decorrelated[k0] = decorrelated[k1];
  305.         decorrelated[k1] = tmp;

  307.     }

  309.     /** Recover ambiguities prior to the Z-transformation.
  310.      * @return recovered ambiguities
  311.      */
  312.     protected IntegerLeastSquareSolution[] recoverAmbiguities() {

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

  316.         int k = 0;
  317.         final long[] a = new long[n];
  318.         for (final IntegerLeastSquareSolution solution : solutions) {
  319.             final long[] s = solution.getSolution();
  320.             for (int i = 0; i < n; ++i) {
  321.                 // compute a = Z⁻ᵀ.s
  322.                 long ai = 0;
  323.                 int l = zIndex(0, i);
  324.                 for (int j = 0; j < n; ++j) {
  325.                     ai += zInverseTransformation[l] * s[j];
  326.                     l  += n;
  327.                 }
  328.                 a[i] = ai;
  329.             }
  330.             recovered[k++] = new IntegerLeastSquareSolution(a, solution.getSquaredDistance());
  331.         }

  333.         return recovered;

  335.     }

  337.     /** Get the index of an entry in the lower triangular matrix.
  338.      * @param row row index (counting from 0)
  339.      * @param col column index (counting from 0)
  340.      * @return index in the single dimension array
  341.      */
  342.     protected int lIndex(final int row, final int col) {
  343.         return (row * (row - 1)) / 2 + col;
  344.     }

  346.     /** Get the index of an entry in the Z transformation matrix.
  347.      * @param row row index (counting from 0)
  348.      * @param col column index (counting from 0)
  349.      * @return index in the single dimension array
  350.      */
  351.     protected int zIndex(final int row, final int col) {
  352.         return row * n + col;
  353.     }

  355. }
Created with JaCoCo 0.8.6.202009150832