DOPComputer

/* Copyright 2002-2021 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.gnss;

import java.util.List;

import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.util.FastMath;
import org.orekit.bodies.GeodeticPoint;
import org.orekit.bodies.OneAxisEllipsoid;
import org.orekit.errors.OrekitException;
import org.orekit.errors.OrekitMessages;
import org.orekit.frames.TopocentricFrame;
import org.orekit.propagation.Propagator;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.ElevationMask;

/**
 * This class aims at computing the dilution of precision.
 *
 * @author Pascal Parraud
 * @since 8.0
 * @see <a href="http://en.wikipedia.org/wiki/Dilution_of_precision_%28GPS%29">Dilution of precision</a>
 */
public class DOPComputer {

    // Constants
    /** Minimum elevation : 0°. */
    public static final double DOP_MIN_ELEVATION = 0.;

    /** Minimum number of propagators for DOP computation. */
    private static final int DOP_MIN_PROPAGATORS = 4;

    // Fields
    /** The location as a topocentric frame. */
    private final TopocentricFrame frame;

    /** Elevation mask used for computation, if defined. */
    private final ElevationMask elevationMask;

    /** Minimum elevation value used if no mask is defined. */
    private final double minElevation;

    /**
     * Constructor for DOP computation.
     *
     * @param frame the topocentric frame linked to the locations where DOP will be computed
     * @param minElev the minimum elevation to consider (rad)
     * @param elevMask the elevation mask to consider
     */
    private DOPComputer(final TopocentricFrame frame, final double minElev, final ElevationMask elevMask) {
        // Set the topocentric frame
        this.frame = frame;
        // Set the min elevation
        this.minElevation = minElev;
        // Set the elevation mask
        this.elevationMask = elevMask;
    }

    /**
     * Creates a DOP computer for one location.
     *
     * <p>A minimum elevation of 0° is taken into account to compute
     * visibility between the location and the GNSS spacecrafts.</p>
     *
     * @param shape the body shape on which the location is defined
     * @param location the point of interest
     * @return a configured DOP computer
     */
    public static DOPComputer create(final OneAxisEllipsoid shape, final GeodeticPoint location) {
        return new DOPComputer(new TopocentricFrame(shape, location, "Location"), DOP_MIN_ELEVATION, null);
    }

    /**
     * Set the minimum elevation.
     *
     * <p>This will override an elevation mask if it has been configured as such previously.</p>
     *
     * @param newMinElevation minimum elevation for visibility (rad)
     * @return a new DOP computer with updated configuration (the instance is not changed)
     *
     * @see #getMinElevation()
     */
    public DOPComputer withMinElevation(final double newMinElevation) {
        return new DOPComputer(frame, newMinElevation, null);
    }

    /**
     * Set the elevation mask.
     *
     * <p>This will override the min elevation if it has been configured as such previously.</p>
     *
     * @param newElevationMask elevation mask to use for the computation
     * @return a new detector with updated configuration (the instance is not changed)
     *
     * @see #getElevationMask()
     */
    public DOPComputer withElevationMask(final ElevationMask newElevationMask) {
        return new DOPComputer(frame, DOP_MIN_ELEVATION, newElevationMask);
    }

    /**
     * Compute the {@link DOP} at a given date for a set of GNSS spacecrafts.
     * <p>Four GNSS spacecraft at least are needed to compute the DOP.
     * If less than 4 propagators are provided, an exception will be thrown.
     * If less than 4 spacecrafts are visible at the date, all DOP values will be
     * set to {@link java.lang.Double#NaN NaN}.</p>
     *
     * @param date the computation date
     * @param gnss the propagators for GNSS spacecraft involved in the DOP computation
     * @return the {@link DOP} at the location
     */
    public DOP compute(final AbsoluteDate date, final List<Propagator> gnss) {

        // Checks the number of provided propagators
        if (gnss.size() < DOP_MIN_PROPAGATORS) {
            throw new OrekitException(OrekitMessages.NOT_ENOUGH_GNSS_FOR_DOP, gnss.size(), DOP_MIN_PROPAGATORS);
        }

        // Initializes DOP values
        double gdop = Double.NaN;
        double pdop = Double.NaN;
        double hdop = Double.NaN;
        double vdop = Double.NaN;
        double tdop = Double.NaN;

        // Loop over the propagators of GNSS orbits
        final double[][] satDir = new double[gnss.size()][4];
        int satNb = 0;
        for (Propagator prop : gnss) {
            final Vector3D pos = prop.getPVCoordinates(date, frame).getPosition();
            final double elev  = frame.getElevation(pos, frame, date);
            final double elMin = (elevationMask != null) ?
                                 elevationMask.getElevation(frame.getAzimuth(pos, frame, date)) :
                                 minElevation;
            // Only visible satellites are considered
            if (elev > elMin) {
                // Create the rows of the H matrix
                final Vector3D r = pos.normalize();
                satDir[satNb][0] = r.getX();
                satDir[satNb][1] = r.getY();
                satDir[satNb][2] = r.getZ();
                satDir[satNb][3] = -1.;
                satNb++;
            }
        }

        // DOP values are computed only if at least 4 SV are visible from the location
        if (satNb > 3) {
            // Construct matrix H
            final RealMatrix h = MatrixUtils.createRealMatrix(satNb, 4);
            for (int k = 0; k < satNb; k++) {
                h.setRow(k, satDir[k]);
            }

            // Compute the pseudo-inverse of H
            final RealMatrix hInv = MatrixUtils.inverse(h.transpose().multiply(h));
            final double sx2 = hInv.getEntry(0, 0);
            final double sy2 = hInv.getEntry(1, 1);
            final double sz2 = hInv.getEntry(2, 2);
            final double st2 = hInv.getEntry(3, 3);

            // Extract various DOP : GDOP, PDOP, HDOP, VDOP, TDOP
            gdop = FastMath.sqrt(hInv.getTrace());
            pdop = FastMath.sqrt(sx2 + sy2 + sz2);
            hdop = FastMath.sqrt(sx2 + sy2);
            vdop = FastMath.sqrt(sz2);
            tdop = FastMath.sqrt(st2);
        }

        // Return all the DOP values
        return new DOP(frame.getPoint(), date, satNb, gdop, pdop, hdop, vdop, tdop);
    }

    /**
     * Get the minimum elevation.
     *
     * @return the minimum elevation (rad)
     */
    public double getMinElevation() {
        return minElevation;
    }

    /**
     * Get the elevation mask.
     *
     * @return the elevation mask
     */
    public ElevationMask getElevationMask() {
        return elevationMask;
    }
}