DOPComputer.java

  1. /* Copyright 2002-2022 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;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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