Package com.irurueta.navigation.gnss

Class GNSSLeastSquaresPositionAndVelocityEstimator

java.lang.Object
com.irurueta.navigation.gnss.GNSSLeastSquaresPositionAndVelocityEstimator
public class GNSSLeastSquaresPositionAndVelocityEstimator extends Object
Calculates position, velocity, clock offset and clock drift using unweighted iterated least squares. Separate calculations are implemented for position and clock offset and for velocity and clock drift. This implementation is based on the equations defined in "Principles of GNSS, Inertial, and Multi-sensor Integrated Navigation Systems, Second Edition" and on the companion software available at: https://github.com/ymjdz/MATLAB-Codes/blob/master/GNSS_LS_position_velocity.m

  • Field Details

    • MIN_MEASUREMENTS

      public static final int MIN_MEASUREMENTS
      Minimum number of measurements required to obtain a solution.
      See Also:

    • CONVERGENCE_THRESHOLD

      public static final double CONVERGENCE_THRESHOLD
      Default threshold to determine when convergence has been reached.
      See Also:

    • SPEED_OF_LIGHT

      public static final double SPEED_OF_LIGHT
      Speed of light in the vacuum expressed in meters per second (m/s).
      See Also:

    • EARTH_ROTATION_RATE

      public static final double EARTH_ROTATION_RATE
      Earth rotation rate expressed in radians per second (rad/s).
      See Also:

    • STATE_COMPONENTS

      private static final int STATE_COMPONENTS
      Number of components of predicted state.
      See Also:

    • ELEMS

      private static final int ELEMS
      Number of elements of position, velocity, etc.
      See Also:

    • ELEMS_MINUS_ONE

      private static final int ELEMS_MINUS_ONE
      Number of elements of position minus one.
      See Also:

    • measurements

      private Collection<GNSSMeasurement> measurements
      GNSS measurements of a collection of satellites.

    • priorPositionAndVelocity

      private ECEFPositionAndVelocity priorPositionAndVelocity
      Previously predicted ECEF user position and velocity.

    • listener

      private GNSSLeastSquaresPositionAndVelocityEstimatorListener listener
      Listener to notify events raised by this instance.

    • convergenceThreshold

      private double convergenceThreshold
      Threshold to determine when convergence has been reached.

    • running

      private boolean running
      Indicates whether estimation is currently running.

    • cei

      private final com.irurueta.algebra.Matrix cei
      Internal matrix to be reused containing frame rotation during signal transit time.

    • xPred

      private final com.irurueta.algebra.Matrix xPred
      Predicted state to be reused.

    • tmp1

      private final com.irurueta.algebra.Matrix tmp1
      Temporal matrix to be reused.

    • tmp2

      private final com.irurueta.algebra.Matrix tmp2
      Temporal matrix to be reused.

    • xEst

      private final com.irurueta.algebra.Matrix xEst
      Estimated state to be reused.

    • hSqr

      private final com.irurueta.algebra.Matrix hSqr
      Contains square representation of measurement or geometry matrix.

    • invHSqr

      private final com.irurueta.algebra.Matrix invHSqr
      Inverse of the square representation of measurement or geometry matrix.

    • tmp3

      private final com.irurueta.algebra.Matrix tmp3
      Temporal matrix to be reused.

    • omegaIe

      private final com.irurueta.algebra.Matrix omegaIe
      Skew symmetric matrix of Earth rotation rate.

    • tmp4

      private final com.irurueta.algebra.Matrix tmp4
      Temporal matrix to be reused.

    • tmp5

      private final com.irurueta.algebra.Matrix tmp5
      Temporal matrix to be reused.

    • tmp6

      private final com.irurueta.algebra.Matrix tmp6
      Temporal matrix to be reused.

    • tmp7

      private final com.irurueta.algebra.Matrix tmp7
      Temporal matrix to be reused.

    • tmp8

      private final com.irurueta.algebra.Matrix tmp8
      Temporal matrix to be reused.

    • tmp9

      private final com.irurueta.algebra.Matrix tmp9
      Temporal matrix to be reused.

    • tmp10

      private final com.irurueta.algebra.Matrix tmp10
      Temporal matrix to be reused.

    • measurementPosition

      private final com.irurueta.algebra.Matrix measurementPosition
      Measurement position to be reused.

    • measurementVelocity

      private final com.irurueta.algebra.Matrix measurementVelocity
      Measurement velocity to be reused.

    • predVelocity

      private final com.irurueta.algebra.Matrix predVelocity
      Predicted velocity to be reused.

    • resultPosition

      private final com.irurueta.algebra.Matrix resultPosition
      Result position to be reused.

  • Constructor Details

    • GNSSLeastSquaresPositionAndVelocityEstimator

      public GNSSLeastSquaresPositionAndVelocityEstimator()
      Constructor.

    • GNSSLeastSquaresPositionAndVelocityEstimator

      public GNSSLeastSquaresPositionAndVelocityEstimator(Collection<GNSSMeasurement> measurements)
      Constructor.
      Parameters:
      measurements - GNSS measurements of a collection of satellites.
      Throws:
      IllegalArgumentException - if less than 4 measurements are provided.

    • GNSSLeastSquaresPositionAndVelocityEstimator

      public GNSSLeastSquaresPositionAndVelocityEstimator(Collection<GNSSMeasurement> measurements, ECEFPositionAndVelocity priorPositionAndVelocity)
      Constructor.
      Parameters:
      measurements - GNSS measurements of a collection of satellites.
      priorPositionAndVelocity - previously predicted ECEF user position and velocity.
      Throws:
      IllegalArgumentException - if less than 4 measurements are provided.

    • GNSSLeastSquaresPositionAndVelocityEstimator

      public GNSSLeastSquaresPositionAndVelocityEstimator(Collection<GNSSMeasurement> measurements, GNSSEstimation priorEstimation)
      Constructor.
      Parameters:
      measurements - GNSS measurement of a collection of satellites.
      priorEstimation - previously predicted GNSS estimation.
      Throws:
      IllegalArgumentException - if less than 4 measurements are provided.

    • GNSSLeastSquaresPositionAndVelocityEstimator

      public GNSSLeastSquaresPositionAndVelocityEstimator(GNSSLeastSquaresPositionAndVelocityEstimatorListener listener)
      Constructor.
      Parameters:
      listener - listener notifying events raised by this instance.

    • GNSSLeastSquaresPositionAndVelocityEstimator

      public GNSSLeastSquaresPositionAndVelocityEstimator(Collection<GNSSMeasurement> measurements, GNSSLeastSquaresPositionAndVelocityEstimatorListener listener)
      Constructor.
      Parameters:
      measurements - GNSS measurements of a collection of satellites.
      listener - listener notifying events raised by this instance.
      Throws:
      IllegalArgumentException - if less than 4 measurements are provided.

    • GNSSLeastSquaresPositionAndVelocityEstimator

      public GNSSLeastSquaresPositionAndVelocityEstimator(Collection<GNSSMeasurement> measurements, ECEFPositionAndVelocity priorPositionAndVelocity, GNSSLeastSquaresPositionAndVelocityEstimatorListener listener)
      Constructor.
      Parameters:
      measurements - GNSS measurements of a collection of satellites.
      priorPositionAndVelocity - previously predicted ECEF user position and velocity.
      listener - listener notifying events raised by this instance.
      Throws:
      IllegalArgumentException - if less than 4 measurements are provided.

    • GNSSLeastSquaresPositionAndVelocityEstimator

      public GNSSLeastSquaresPositionAndVelocityEstimator(Collection<GNSSMeasurement> measurements, GNSSEstimation priorEstimation, GNSSLeastSquaresPositionAndVelocityEstimatorListener listener)
      Constructor.
      Parameters:
      measurements - GNSS measurement of a collection of satellites.
      priorEstimation - previously predicted GNSS estimation.
      listener - listener notifying events raised by this instance.
      Throws:
      IllegalArgumentException - if less than 4 measurements are provided.

  • Method Details

    • getMeasurements

      public Collection<GNSSMeasurement> getMeasurements()
      Gets GNSS measurements of a collection of satellites.
      Returns:
      GNSS measurements of a collection of satellites.

    • setMeasurements

      public void setMeasurements(Collection<GNSSMeasurement> measurements) throws LockedException
      Sets GNSS measurements of a collection of satellites.
      Parameters:
      measurements - GNSS measurements of a collection of satellites.
      Throws:
      IllegalArgumentException - if less than 4 measurements are provided.
      LockedException - if this estimator is already running.

    • getPriorPositionAndVelocity

      public ECEFPositionAndVelocity getPriorPositionAndVelocity()
      Gets previously predicted ECEF user position and velocity.
      Returns:
      previously predicted ECEF user position and velocity.

    • setPriorPositionAndVelocity

      public void setPriorPositionAndVelocity(ECEFPositionAndVelocity priorPositionAndVelocity) throws LockedException
      Sets previously predicted ECEF user position and velocity.
      Parameters:
      priorPositionAndVelocity - previously predicted ECEF user position and velocity.
      Throws:
      LockedException - if this estimator is already running.

    • setPriorPositionAndVelocityFromEstimation

      public void setPriorPositionAndVelocityFromEstimation(GNSSEstimation priorEstimation) throws LockedException
      Sets previously predicted ECEF user position and velocity from a previous predicted result.
      Parameters:
      priorEstimation - previously predicted GNSS estimation.
      Throws:
      LockedException - if this estimator is already running.

    • getListener

      public GNSSLeastSquaresPositionAndVelocityEstimatorListener getListener()
      Gets listener to notify events raised by this instance.
      Returns:
      listener to notify events raised by this instance.

    • setListener

      public void setListener(GNSSLeastSquaresPositionAndVelocityEstimatorListener listener) throws LockedException
      Sets listener to notify events raised by this instance.
      Parameters:
      listener - listener to notify events raised by this instance.
      Throws:
      LockedException - if this estimator is already running.

    • getConvergenceThreshold

      public double getConvergenceThreshold()
      Gets threshold to determine when convergence has been reached.
      Returns:
      threshold to determine when convergence has been reached.

    • setConvergenceThreshold

      public void setConvergenceThreshold(double convergenceThreshold) throws LockedException, IllegalArgumentException
      Sets threshold to determine when convergence has been reached.
      Parameters:
      convergenceThreshold - threshold to determine when convergence has been reached.
      Throws:
      LockedException - if this estimator is already running.
      IllegalArgumentException - if provided threshold is zero or negative.

    • isReady

      public boolean isReady()
      Indicates whether this estimator is ready to start the estimation.
      Returns:
      true if estimator is ready, false otherwise.

    • isRunning

      public boolean isRunning()
      Indicates whether this estimator is currently running or not.
      Returns:
      true if estimator is running, false otherwise.

    • isValidMeasurements

      public static boolean isValidMeasurements(Collection<GNSSMeasurement> gnssMeasurements)
      Indicates whether provided measurements are valid or not.
      Parameters:
      gnssMeasurements - measurements to be checked.
      Returns:
      true if at least 4 measurements are provided, false otherwise.

    • estimate

      public void estimate(GNSSEstimation result) throws NotReadyException, LockedException, GNSSException
      Estimates new ECEF user position and velocity as well as clock offset and drift.
      Parameters:
      result - instance where result data will be stored.
      Throws:
      NotReadyException - if estimator is not ready to start estimation.
      LockedException - if estimator is already running.
      GNSSException - if estimation fails due to numerical instabilities.

    • estimate

      public GNSSEstimation estimate() throws NotReadyException, LockedException, GNSSException
      Estimates new ECEF user position and velocity as well as clock offset and drift.
      Returns:
      new ECEF user position and velocity, and clock offset and drift.
      Throws:
      NotReadyException - if estimator is not ready to start estimation.
      LockedException - if estimator is already running.
      GNSSException - if estimation fails due to numerical instabilities.

    • initializePriorPositionAndVelocityIfNeeded

      private void initializePriorPositionAndVelocityIfNeeded()
      Initializes prior position and velocity if not set, assuming that user is located at the average latitude, longitude of all provided satellite measurements, at Earth's surface (height = 0) and with zero velocity.

    • norm

      private static double norm(double x, double y, double z)
      Computes norm of provided coordinates.
      Parameters:
      x - x coordinate.
      y - y coordinate.
      z - z coordinate.
      Returns:
      computed norm.

    • predictionError

      private double predictionError()
      Computes norm of error between estimated state and predicted state.
      Returns:
      norm of error.