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;

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

  20. import org.hipparchus.linear.RealMatrix;
  21. import org.hipparchus.util.FastMath;

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

  35.     /** Number of ambiguities. */
  36.     private int n;

  37.     /** Decorrelated ambiguities. */
  38.     private double[] decorrelated;

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

  41.     /** Diagonal matrix. */
  42.     private double[] diag;

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

  45.     /** Maximum number of solutions seeked. */
  46.     private int maxSolutions;

  47.     /** Placeholder for solutions found. */
  48.     private SortedSet<IntegerLeastSquareSolution> solutions;

  49.     /** {@inheritDoc} */
  50.     @Override
  51.     public IntegerLeastSquareSolution[] solveILS(final int nbSol, final double[] floatAmbiguities,
  52.                                                  final int[] indirection, final RealMatrix covariance) {

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

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

  57.         // perform decorrelation/reduction of covariances
  58.         reduction();

  59.         // transform the Lᵀ.D.L = Q decomposition of covariance into
  60.         // the L⁻¹.D⁻¹.L⁻ᵀ = Q⁻¹ decomposition of the inverse of covariance.
  61.         inverseDecomposition();

  62.         // perform discrete search of Integer Least Square problem
  63.         discreteSearch();

  64.         return recoverAmbiguities();

  65.     }

  66.     /** Initialize ILS problem.
  67.      * @param floatAmbiguities float estimates of ambiguities
  68.      * @param indirection indirection array to extract ambiguity covariances from global covariance matrix
  69.      * @param globalCovariance global covariance matrix (includes ambiguities among other parameters)
  70.      * @param nbSol number of solutions to search for
  71.      */
  72.     private void initializeProblem(final double[] floatAmbiguities, final int[] indirection,
  73.                                    final RealMatrix globalCovariance, final int nbSol) {

  74.         this.n                      = floatAmbiguities.length;
  75.         this.decorrelated           = floatAmbiguities.clone();
  76.         this.low                    = new double[(n * (n - 1)) / 2];
  77.         this.diag                   = new double[n];
  78.         this.zInverseTransformation = new int[n * n];
  79.         this.maxSolutions           = nbSol;
  80.         this.solutions              = new TreeSet<>();

  81.         // initialize decomposition matrices
  82.         for (int i = 0; i < n; ++i) {
  83.             for (int j = 0; j < i; ++j) {
  84.                 low[lIndex(i, j)] = globalCovariance.getEntry(indirection[i], indirection[j]);
  85.             }
  86.             diag[i] = globalCovariance.getEntry(indirection[i], indirection[i]);
  87.             zInverseTransformation[zIndex(i, i)] = 1;
  88.         }

  89.     }

  90.     /** Get a reference to the diagonal matrix of the decomposition.
  91.      * <p>
  92.      * BEWARE: the returned value is a reference to an internal array,
  93.      * it is <em>only</em> intended for subclasses use (hence the
  94.      * method is protected and not public).
  95.      * </p>
  96.      * @return reference to the diagonal matrix of the decomposition
  97.      */
  98.     protected double[] getDiagReference() {
  99.         return diag;
  100.     }

  101.     /** Get a reference to the lower triangular matrix of the decomposition.
  102.      * <p>
  103.      * BEWARE: the returned value is a reference to an internal array,
  104.      * it is <em>only</em> intended for subclasses use (hence the
  105.      * method is protected and not public).
  106.      * </p>
  107.      * @return reference to the lower triangular matrix of the decomposition
  108.      */
  109.     protected double[] getLowReference() {
  110.         return low;
  111.     }

  112.     /** Get the reference decorrelated ambiguities.
  113.      * @return reference to the decorrelated ambiguities.
  114.      **/
  115.     protected double[] getDecorrelatedReference() {
  116.         return decorrelated;
  117.     }

  118.     /** Get the maximum number of solutions seeked.
  119.      * @return the maximum number of solutions seeked
  120.      */
  121.     protected int getMaxSolution() {
  122.         return maxSolutions;
  123.     }

  124.     /** Add a new solution.
  125.      * @param fixed solution array
  126.      * @param squaredNorm squared distance to the corresponding float solution
  127.      */
  128.     protected void addSolution(final long[] fixed, final double squaredNorm) {
  129.         solutions.add(new IntegerLeastSquareSolution(fixed, squaredNorm));
  130.     }

  131.     /** Remove spurious solution.
  132.      */
  133.     protected void removeSolution() {
  134.         solutions.remove(solutions.last());
  135.     }

  136.     /** Get the number of solutions found.
  137.      * @return the number of solutions found
  138.      */
  139.     protected int getSolutionsSize() {
  140.         return solutions.size();
  141.     }

  142.     /** Perform Lᵀ.D.L = Q decomposition of the covariance matrix.
  143.      */
  144.     protected abstract void ltdlDecomposition();

  145.     /** Perform LAMBDA reduction.
  146.      */
  147.     protected abstract void reduction();

  148.     /** Find the best solutions to the Integer Least Square problem.
  149.      */
  150.     protected abstract void discreteSearch();

  151.     /** Inverse the decomposition.
  152.      * <p>
  153.      * This method transforms the Lᵀ.D.L = Q decomposition of covariance into
  154.      * the L⁻¹.D⁻¹.L⁻ᵀ = Q⁻¹ decomposition of the inverse of covariance.
  155.      * </p>
  156.      */
  157.     protected abstract void inverseDecomposition();

  158.     /** Perform one integer Gauss transformation.
  159.      * <p>
  160.      * This method corresponds to algorithm 2.1 in X.-W Chang, X. Yang and T. Zhou paper.
  161.      * </p>
  162.      * @param row row index (counting from 0)
  163.      * @param col column index (counting from 0)
  164.      */
  165.     protected void integerGaussTransformation(final int row, final int col) {
  166.         final int mu = (int) FastMath.rint(low[lIndex(row, col)]);
  167.         if (mu != 0) {

  168.             // update low triangular matrix (post-multiplying L by Zᵢⱼ = I - μ eᵢ eⱼᵀ)
  169.             low[lIndex(row, col)] -= mu;
  170.             for (int i = row + 1; i < n; ++i) {
  171.                 low[lIndex(i, col)] -= mu * low[lIndex(i, row)];
  172.             }

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

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

  180.         }
  181.     }

  182.     /** Perform one symmetric permutation involving rows/columns {@code k0} and {@code k0+1}.
  183.      * <p>
  184.      * This method corresponds to algorithm 2.2 in X.-W Chang, X. Yang and T. Zhou paper.
  185.      * </p>
  186.      * @param k0 diagonal index (counting from 0)
  187.      * @param delta new value for diag[k0+1]
  188.      */
  189.     protected void permutation(final int k0, final double delta) {

  190.         final int    k1        = k0 + 1;
  191.         final int    k2        = k0 + 2;
  192.         final int    indexk1k0 = lIndex(k1, k0);
  193.         final double lk1k0     = low[indexk1k0];
  194.         final double eta       = diag[k0] / delta;
  195.         final double lambda    = diag[k1] * lk1k0 / delta;

  196.         // update diagonal
  197.         diag[k0] = eta * diag[k1];
  198.         diag[k1] = delta;

  199.         // update low triangular matrix
  200.         for (int j = 0; j < k0; ++j) {
  201.             final int indexk0j = lIndex(k0, j);
  202.             final int indexk1j = lIndex(k1, j);
  203.             final double lk0j  = low[indexk0j];
  204.             final double lk1j  = low[indexk1j];
  205.             low[indexk0j]      = lk1j          - lk1k0 * lk0j;
  206.             low[indexk1j]      = lambda * lk1j + eta   * lk0j;
  207.         }
  208.         low[indexk1k0] = lambda;
  209.         for (int i = k2; i < n; ++i) {
  210.             final int indexik0 = lIndex(i, k0);
  211.             final int indexik1 = indexik0 + 1;
  212.             final double tmp   = low[indexik0];
  213.             low[indexik0]      = low[indexik1];
  214.             low[indexik1]      = tmp;
  215.         }

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

  218.             final int indexk0i               = zIndex(k0, i);
  219.             final int indexk1i               = indexk0i + n;
  220.             final int tmp2                   = zInverseTransformation[indexk0i];
  221.             zInverseTransformation[indexk0i] = zInverseTransformation[indexk1i];
  222.             zInverseTransformation[indexk1i] = tmp2;

  223.         }

  224.         // update decorrelated ambiguities
  225.         final double tmp = decorrelated[k0];
  226.         decorrelated[k0] = decorrelated[k1];
  227.         decorrelated[k1] = tmp;

  228.     }

  229.     /** Recover ambiguities prior to the Z-transformation.
  230.      * @return recovered ambiguities
  231.      */
  232.     private IntegerLeastSquareSolution[] recoverAmbiguities() {

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

  234.         int k = 0;
  235.         final long[] a = new long[n];
  236.         for (final IntegerLeastSquareSolution solution : solutions) {
  237.             final long[] s = solution.getSolution();
  238.             for (int i = 0; i < n; ++i) {
  239.                 // compute a = Z⁻ᵀ.s
  240.                 long ai = 0;
  241.                 int l = zIndex(0, i);
  242.                 for (int j = 0; j < n; ++j) {
  243.                     ai += zInverseTransformation[l] * s[j];
  244.                     l  += n;
  245.                 }
  246.                 a[i] = ai;
  247.             }
  248.             recovered[k++] = new IntegerLeastSquareSolution(a, solution.getSquaredDistance());
  249.         }

  250.         return recovered;

  251.     }

  252.     /** Get the index of an entry in the lower triangular matrix.
  253.      * @param row row index (counting from 0)
  254.      * @param col column index (counting from 0)
  255.      * @return index in the single dimension array
  256.      */
  257.     protected int lIndex(final int row, final int col) {
  258.         return (row * (row - 1)) / 2 + col;
  259.     }

  260.     /** Get the index of an entry in the Z transformation matrix.
  261.      * @param row row index (counting from 0)
  262.      * @param col column index (counting from 0)
  263.      * @return index in the single dimension array
  264.      */
  265.     protected int zIndex(final int row, final int col) {
  266.         return row * n + col;
  267.     }

  268. }