LambdaMethod.java

/* Copyright 2002-2022 CS GROUP
 * Licensed to CS GROUP (CS) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * CS licenses this file to You under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with
 * the License.  You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
package org.orekit.estimation.measurements.gnss;

import java.util.SortedSet;
import java.util.TreeSet;

import org.hipparchus.util.FastMath;

/** Decorrelation/reduction engine for LAMBDA method.
 * <p>
 * This class implements PJG Teunissen Least Square Ambiguity Decorrelation
 * Adjustment (LAMBDA) method, as described in both the 1996 paper <a
 * href="https://www.researchgate.net/publication/2790708_The_LAMBDA_method_for_integer_ambiguity_estimation_implementation_aspects">
 * The LAMBDA method for integer ambiguity estimation: implementation aspects</a> by
 * Paul de Jonge and Christian Tiberius and on the 2005 paper <a
 * href="https://www.researchgate.net/publication/225518977_MLAMBDA_a_modified_LAMBDA_method_for_integer_least-squares_estimation">
 * A modified LAMBDA method for integer least-squares estimation</a> by X.-W Chang, X. Yang
 * and T. Zhou, Journal of Geodesy 79(9):552-565, DOI: 10.1007/s00190-005-0004-x
 * </p>
 * <p>
 * It slightly departs on the original LAMBDA method as it does implement
 * the following improvements proposed in the de Jonge and Tiberius 1996 paper
 * that vastly speed up the search:
 * </p>
 * <ul>
 *   <li>alternate search starting from the middle and expanding outwards</li>
 *   <li>automatic shrinking of ellipsoid during the search</li>
 * </ul>
 * @see AmbiguitySolver
 * @author Luc Maisonobe
 * @since 10.0
 */
public class LambdaMethod extends AbstractLambdaMethod {

    /** Margin factor to apply to estimated search limit parameter. */
    private static final double CHI2_MARGIN_FACTOR = 1.1;

    /** {@inheritDoc} */
    @Override
    protected void ltdlDecomposition() {

        // get references to the diagonal and lower triangular parts
        final double[] diag = getDiagReference();
        final double[] low  = getLowReference();

        // perform Lᵀ.D.L decomposition
        for (int k = diag.length - 1; k >= 0; --k) {
            final double inv = 1.0 / diag[k];
            for (int i = 0; i < k; ++i) {
                final double lki = low[lIndex(k, i)];
                for (int j = 0; j < i; ++j) {
                    low[lIndex(i, j)] -= lki * low[lIndex(k, j)] * inv;
                }
                diag[i] -= lki * lki * inv;
            }
            for (int j = 0; j < k; ++j) {
                low[lIndex(k, j)] *= inv;
            }
        }

    }

    /** {@inheritDoc} */
    @Override
    protected void reduction() {

        // get references to the diagonal and lower triangular parts
        final double[] diag = getDiagReference();
        final double[] low  = getLowReference();
        final int n = diag.length;

        int kSaved = n - 2;
        int k0 = kSaved;
        while (k0 >= 0) {
            final int k1 = k0 + 1;
            if (k0 <= kSaved) {
                for (int i = k0 + 1; i < n; ++i) {
                    integerGaussTransformation(i, k0);
                }
            }
            final double lk1k0 = low[lIndex(k1, k0)];
            final double delta = diag[k0] + lk1k0 * lk1k0 * diag[k1];
            if (delta < diag[k1]) {
                permutation(k0, delta);
                kSaved = k0;
                k0 = n - 2;
            } else {
                k0--;
            }
        }
    }

    /** {@inheritDoc} */
    @Override
    protected void discreteSearch() {

        // get references to the diagonal part
        final double[] diag = getDiagReference();
        final int n = diag.length;

        // estimate search domain limit
        final long[]   fixed   = new long[n];
        final double[] offsets = new double[n];
        final double[] left    = new double[n];
        final double[] right   = new double[n];

        final int maxSolutions = getMaxSolution();
        final double[] decorrelated = getDecorrelatedReference();

        final AlternatingSampler[] samplers = new AlternatingSampler[n];

        // set up top level sampling for last ambiguity
        double chi2 = estimateChi2(fixed, offsets);
        right[n - 1] = chi2 / diag[n - 1];
        int index = n - 1;
        while (index < n) {
            if (index == -1) {

                // all ambiguities have been fixed
                final double squaredNorm = chi2 - diag[0] * (right[0] - left[0]);
                addSolution(fixed, squaredNorm);
                final int size = getSolutionsSize();
                if (size >= maxSolutions) {

                    // shrink the ellipsoid
                    chi2 = squaredNorm;
                    right[n - 1] = chi2 / diag[n - 1];
                    for (int i = n - 1; i > 0; --i) {
                        samplers[i].setRadius(FastMath.sqrt(right[i]));
                        right[i - 1] = diag[i] / diag[i - 1] * (right[i] - left[i]);
                    }
                    samplers[0].setRadius(FastMath.sqrt(right[0]));

                    if (size > maxSolutions) {
                        removeSolution();
                    }

                }

                ++index;

            } else {

                if (samplers[index] == null) {
                    // we start exploring a new ambiguity
                    samplers[index] = new AlternatingSampler(conditionalEstimate(index, offsets), FastMath.sqrt(right[index]));
                } else {
                    // continue exploring the same ambiguity
                    samplers[index].generateNext();
                }

                if (samplers[index].inRange()) {
                    fixed[index]       = samplers[index].getCurrent();
                    offsets[index]     = fixed[index] - decorrelated[index];
                    final double delta = fixed[index] - samplers[index].getMidPoint();
                    left[index]        = delta * delta;
                    if (index > 0) {
                        right[index - 1]   = diag[index] / diag[index - 1] * (right[index] - left[index]);
                    }

                    // go down one level
                    --index;

                } else {

                    // we have completed exploration of this ambiguity range
                    samplers[index] = null;

                    // go up one level
                    ++index;

                }

            }
        }

    }

    /** {@inheritDoc} */
    @Override
    protected void inverseDecomposition() {

        // get references to the diagonal and lower triangular parts
        final double[] diag = getDiagReference();
        final double[] low  = getLowReference();
        final int n = diag.length;

        // we rely on the following equation, where a low triangular
        // matrix L of dimension n is split into sub-matrices of dimensions
        // k, l and m with k + l + m = n
        //
        // [  A  |      |    ]  [        A⁻¹        |         |       ]   [  Iₖ  |      |     ]
        // [  B  |  Iₗ  |    ]  [       -BA⁻¹       |   Iₗ    |       ] = [      |  Iₗ  |     ]
        // [  C  |  D   |  E ]  [ E⁻¹ (DB - C) A⁻¹  | -E⁻¹D   |  E⁻¹  ]   [      |      |  Iₘ ]
        //
        // considering we have already computed A⁻¹ (i.e. inverted rows 0 to k-1
        // of L), and using l = 1 in the previous expression (i.e. the middle matrix
        // is only one row), we see that elements 0 to k-1 of row k are given by -BA⁻¹
        // and that element k is I₁ = 1. We can therefore invert L row by row and we
        // obtain an inverse matrix L⁻¹ which is a low triangular matrix with unit
        // diagonal. A⁻¹ is therefore also a low triangular matrix with unit diagonal.
        // This property is used in the loops below to speed up the computation of -BA⁻¹
        final double[] row = new double[n - 1];
        diag[0] = 1.0 / diag[0];
        for (int k = 1; k < n; ++k) {

            // compute row k of lower triangular part, by computing -BA⁻¹
            final int iK = lIndex(k, 0);
            System.arraycopy(low, iK, row, 0, k);
            for (int j = 0; j < k; ++j) {
                double sum = row[j];
                for (int l = j + 1; l < k; ++l) {
                    sum += row[l] * low[lIndex(l, j)];
                }
                low[iK + j] = -sum;
            }

            // diagonal part
            diag[k] = 1.0 / diag[k];

        }

    }

    /** Compute a safe estimate of search limit parameter χ².
     * <p>
     * The estimate is based on section 4.11 in de Jonge and Tiberius 1996,
     * computing χ² such that it includes at least a few preset grid points
     * </p>
     * @param fixed placeholder for test fixed ambiguities
     * @param offsets placeholder for offsets between fixed ambiguities and float ambiguities
     * @return safe estimate of search limit parameter χ²
     */
    private double estimateChi2(final long[] fixed, final double[] offsets) {

        // get references to the diagonal part
        final double[] diag = getDiagReference();
        final int n = diag.length;
        // maximum number of solutions seeked
        final int maxSolutions = getMaxSolution();
        // get references to the decorrelated ambiguities
        final double[] decorrelated = getDecorrelatedReference();


        // initialize test points, assuming ambiguities have been fully decorrelated
        final AlternatingSampler[] samplers = new AlternatingSampler[n];
        for (int i = 0; i < n; ++i) {
            samplers[i] = new AlternatingSampler(decorrelated[i], 0.0);
        }

        final SortedSet<Double> squaredNorms = new TreeSet<>();

        // first test point at center
        for (int i = 0; i < n; ++i) {
            fixed[i] = samplers[i].getCurrent();
        }
        squaredNorms.add(squaredNorm(fixed, offsets));

        while (squaredNorms.size() < maxSolutions) {
            // add a series of grid points, each shifted from center along a different axis
            final int remaining = FastMath.min(n, maxSolutions - squaredNorms.size());
            for (int i = 0; i < remaining; ++i) {
                final long saved = fixed[i];
                samplers[i].generateNext();
                fixed[i] = samplers[i].getCurrent();
                squaredNorms.add(squaredNorm(fixed, offsets));
                fixed[i] = saved;
            }
        }

        // select a limit ensuring at least the needed number of grid points are in the search domain
        int count = 0;
        for (final double s : squaredNorms) {
            if (++count == maxSolutions) {
                return s * CHI2_MARGIN_FACTOR;
            }
        }

        // never reached
        return Double.NaN;

    }

    /** Compute squared norm of a set of fixed ambiguities.
     * @param fixed fixed ambiguities
     * @param offsets placeholder for offsets between fixed ambiguities and float ambiguities
     * @return squared norm of a set of fixed ambiguities
     */
    private double squaredNorm(final long[] fixed, final double[] offsets) {
        // get references to the lower triangular part and the decorrelated ambiguities
        final double[] diag = getDiagReference();
        final double[] decorrelated = getDecorrelatedReference();
        double n2 = 0;
        for (int i = diag.length - 1; i >= 0; --i) {
            offsets[i] = fixed[i] - decorrelated[i];
            final double delta = fixed[i] - conditionalEstimate(i, offsets);
            n2 += diag[i] * delta * delta;
        }
        return n2;
    }

    /** Compute conditional estimate of an ambiguity.
     * <p>
     * This corresponds to equation 4.4 in de Jonge and Tiberius 1996
     * </p>
     * @param i index of the ambiguity
     * @param offsets offsets between already fixed ambiguities and float ambiguities
     * @return conditional estimate of ambiguity â<sub>i|i+1...n</sub>
     */
    private double conditionalEstimate(final int i, final double[] offsets) {
        // get references to the diagonal and lower triangular parts
        final double[] diag = getDiagReference();
        final double[] low  = getLowReference();
        // get references to the decorrelated ambiguities
        final double[] decorrelated = getDecorrelatedReference();

        double conditional = decorrelated[i];
        for (int j = i + 1; j < diag.length; ++j) {
            conditional -= low[lIndex(j, i)] * offsets[j];
        }
        return conditional;
    }

}