DOPComputer.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.gnss;

  19. import java.util.List;

  21. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  22. import org.hipparchus.linear.MatrixUtils;
  23. import org.hipparchus.linear.RealMatrix;
  24. import org.hipparchus.util.FastMath;
  25. import org.orekit.bodies.GeodeticPoint;
  26. import org.orekit.bodies.OneAxisEllipsoid;
  27. import org.orekit.errors.OrekitException;
  28. import org.orekit.errors.OrekitMessages;
  29. import org.orekit.frames.TopocentricFrame;
  30. import org.orekit.propagation.Propagator;
  31. import org.orekit.time.AbsoluteDate;
  32. import org.orekit.utils.ElevationMask;
  33. import org.orekit.utils.TrackingCoordinates;

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

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

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

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

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

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

  61.     /**
  62.      * Constructor for DOP computation.
  63.      *
  64.      * @param frame the topocentric frame linked to the locations where DOP will be computed
  65.      * @param minElev the minimum elevation to consider (rad)
  66.      * @param elevMask the elevation mask to consider
  67.      */
  68.     private DOPComputer(final TopocentricFrame frame, final double minElev, final ElevationMask elevMask) {
  69.         // Set the topocentric frame
  70.         this.frame = frame;
  71.         // Set the min elevation
  72.         this.minElevation = minElev;
  73.         // Set the elevation mask
  74.         this.elevationMask = elevMask;
  75.     }

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

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

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

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

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

  137.         // Initializes DOP values
  138.         double gdop = Double.NaN;
  139.         double pdop = Double.NaN;
  140.         double hdop = Double.NaN;
  141.         double vdop = Double.NaN;
  142.         double tdop = Double.NaN;

  144.         // Loop over the propagators of GNSS orbits
  145.         final double[][] satDir = new double[gnss.size()][4];
  146.         int satNb = 0;
  147.         for (Propagator prop : gnss) {
  148.             final Vector3D            pos   = prop.getPosition(date, frame);
  149.             final TrackingCoordinates tc    = frame.getTrackingCoordinates(pos, frame, date);
  150.             final double              elMin = (elevationMask != null) ?
  151.                                               elevationMask.getElevation(tc.getAzimuth()) :
  152.                                                   minElevation;
  153.             // Only visible satellites are considered
  154.             if (tc.getElevation() > elMin) {
  155.                 // Create the rows of the H matrix
  156.                 final Vector3D r = pos.normalize();
  157.                 satDir[satNb][0] = r.getX();
  158.                 satDir[satNb][1] = r.getY();
  159.                 satDir[satNb][2] = r.getZ();
  160.                 satDir[satNb][3] = -1.;
  161.                 satNb++;
  162.             }
  163.         }

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

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

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

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

  192.     /**
  193.      * Get the minimum elevation.
  194.      *
  195.      * @return the minimum elevation (rad)
  196.      */
  197.     public double getMinElevation() {
  198.         return minElevation;
  199.     }

  201.     /**
  202.      * Get the elevation mask.
  203.      *
  204.      * @return the elevation mask
  205.      */
  206.     public ElevationMask getElevationMask() {
  207.         return elevationMask;
  208.     }
  209. }
Created with JaCoCo 0.8.12.202403310830