Package com.irurueta.navigation.gnss

Class GNSSKalmanFilteredEstimator

java.lang.Object
com.irurueta.navigation.gnss.GNSSKalmanFilteredEstimator
public class GNSSKalmanFilteredEstimator extends Object
Calculates position, velocity, clock offset and clock drift using an unweighted iterated least squares estimator along with a Kalman filter to smooth results. 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_Kalman_Filter.m

  • Field Details

    • lsEstimator

      private final GNSSLeastSquaresPositionAndVelocityEstimator lsEstimator
      Internal estimator to compute least squares solution for GNSS measurements.

    • listener

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

    • epochInterval

      private double epochInterval
      Minimum epoch interval expressed in seconds (s) between consecutive propagations or measurements. Attempting to propagate results using Kalman filter or updating measurements when intervals are lass than this value, will be ignored.

    • config

      private GNSSKalmanConfig config
      GNSS Kalman filter configuration parameters (usually obtained through calibration).

    • measurements

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

    • estimation

      private GNSSEstimation estimation
      Current estimation containing user ECEF position, user ECEF velocity, clock offset and clock drift.

    • state

      private GNSSKalmanState state
      Current Kalman filter state containing current GNSS estimation along with Kalman filter covariance error matrix.

    • lastStateTimestamp

      private Double lastStateTimestamp
      Timestamp expressed in seconds since epoch time when Kalman filter state was last propagated.

    • running

      private boolean running
      Indicates whether this estimator is running or not.

  • Constructor Details

    • GNSSKalmanFilteredEstimator

      public GNSSKalmanFilteredEstimator()
      Constructor.

    • GNSSKalmanFilteredEstimator

      public GNSSKalmanFilteredEstimator(GNSSKalmanConfig config)
      Constructor.
      Parameters:
      config - GNSS Kalman filter configuration parameters (usually obtained through calibration).

    • GNSSKalmanFilteredEstimator

      public GNSSKalmanFilteredEstimator(double epochInterval)
      Constructor.
      Parameters:
      epochInterval - minimum epoch interval expressed in seconds (s) between consecutive propagations or measurements.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • GNSSKalmanFilteredEstimator

      public GNSSKalmanFilteredEstimator(GNSSKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      listener - listener to notify events raised by this instance.

    • GNSSKalmanFilteredEstimator

      public GNSSKalmanFilteredEstimator(GNSSKalmanConfig config, double epochInterval)
      Constructor.
      Parameters:
      config - GNSS Kalman filter configuration parameters (usually obtained through calibration).
      epochInterval - minimum epoch interval expressed in seconds (s) between consecutive propagations or measurements.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • GNSSKalmanFilteredEstimator

      public GNSSKalmanFilteredEstimator(GNSSKalmanConfig config, GNSSKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      config - GNSS Kalman filter configuration parameters (usually obtained through calibration).
      listener - listener to notify events raised by this instance.

    • GNSSKalmanFilteredEstimator

      public GNSSKalmanFilteredEstimator(double epochInterval, GNSSKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      epochInterval - minimum epoch interval expressed in seconds (s) between consecutive propagations or measurements.
      listener - listener to notify events raised by this instance.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • GNSSKalmanFilteredEstimator

      public GNSSKalmanFilteredEstimator(GNSSKalmanConfig config, double epochInterval, GNSSKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      config - GNSS Kalman filter configuration parameters (usually obtained through calibration).
      epochInterval - minimum epoch interval expressed in seconds (s) between consecutive propagations or measurements.
      listener - listener to notify events raised by this instance.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • GNSSKalmanFilteredEstimator

      public GNSSKalmanFilteredEstimator(com.irurueta.units.Time epochInterval)
      Constructor.
      Parameters:
      epochInterval - minimum epoch interval between consecutive propagations or measurements.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • GNSSKalmanFilteredEstimator

      public GNSSKalmanFilteredEstimator(GNSSKalmanConfig config, com.irurueta.units.Time epochInterval)
      Constructor.
      Parameters:
      config - GNSS Kalman filter configuration parameters (usually obtained through calibration).
      epochInterval - minimum epoch interval between consecutive propagations or measurements.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • GNSSKalmanFilteredEstimator

      public GNSSKalmanFilteredEstimator(com.irurueta.units.Time epochInterval, GNSSKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      epochInterval - minimum epoch interval between consecutive propagations or measurements.
      listener - listener to notify events raised by this instance.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • GNSSKalmanFilteredEstimator

      public GNSSKalmanFilteredEstimator(GNSSKalmanConfig config, com.irurueta.units.Time epochInterval, GNSSKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      config - GNSS Kalman filter configuration parameters (usually obtained through calibration).
      epochInterval - minimum epoch interval between consecutive propagations or measurements.
      listener - listener to notify events raised by this instance.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

  • Method Details

    • getListener

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

    • setListener

      public void setListener(GNSSKalmanFilteredEstimatorListener 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.

    • getEpochInterval

      public double getEpochInterval()
      Gets minimum epoch interval expressed in seconds (s) between consecutive propagations or measurements expressed in seconds. Attempting to propagate results using Kalman filter or updating measurements when intervals are less than this value, will be ignored.
      Returns:
      minimum epoch interval between consecutive propagations or measurements.

    • setEpochInterval

      public void setEpochInterval(double epochInterval) throws LockedException
      Sets minimum epoch interval expressed in seconds (s) between consecutive propagations or measurements expressed in seconds. Attempting to propagate results using Kalman filter or updating measurements when intervals are less than this value, will be ignored.
      Parameters:
      epochInterval - minimum epoch interval expressed in seconds (s) between consecutive propagations or measurements.
      Throws:
      LockedException - if this estimator is already running.
      IllegalArgumentException - if provided epoch interval is negative.

    • getEpochIntervalAsTime

      public void getEpochIntervalAsTime(com.irurueta.units.Time result)
      Gets minimum epoch interval between consecutive propagations or measurements. Attempting to propagate results using Kalman filter or updating measurements when intervals are less than this value, will be ignored.
      Parameters:
      result - instance where minimum epoch interval will be stored.

    • getEpochIntervalAsTime

      public com.irurueta.units.Time getEpochIntervalAsTime()
      Gets minimum epoch interval between consecutive propagations or measurements. Attempting to propagate results using Kalman filter or updating measurements when intervals are less than this value, will be ignored.
      Returns:
      minimum epoch interval.

    • setEpochInterval

      public void setEpochInterval(com.irurueta.units.Time epochInterval) throws LockedException
      Sets minimum epoch interval between consecutive propagations or measurements. Attempting to propagate results using Kalman filter or updating measurements when intervals are less than this value, will be ignored.
      Parameters:
      epochInterval - minimum epoch interval.
      Throws:
      LockedException - if this estimator is already running.
      IllegalArgumentException - if provided epoch interval is negative.

    • getConfig

      public boolean getConfig(GNSSKalmanConfig result)
      Gets GNSS Kalman configuration parameters (usually obtained through calibration).
      Parameters:
      result - instance where GNSS Kalman configuration parameters will be stored.
      Returns:
      true if result instance is updated, false otherwise.

    • getConfig

      public GNSSKalmanConfig getConfig()
      Gets GNSS Kalman configuration parameters (usually obtained through calibration).
      Returns:
      GNSS Kalman configuration parameters.

    • setConfig

      public void setConfig(GNSSKalmanConfig config) throws LockedException
      Sets GNSS Kalman configuration parameters (usually obtained through calibration).
      Parameters:
      config - GNSS Kalman configuration parameters to be set.
      Throws:
      LockedException - if this estimator is already running.

    • getMeasurements

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

    • getEstimation

      public GNSSEstimation getEstimation()
      Gets current estimation containing user ECEF position, user ECEF velocity, clock offset and clock drift.
      Returns:
      current estimation containing user ECEF position, user ECEF velocity, clock offset and clock drift.

    • getEstimation

      public boolean getEstimation(GNSSEstimation result)
      Gets current estimation containing user ECEF position, user ECEF velocity, clock offset and clock drift. This method does not update result instance if no estimation is available.
      Parameters:
      result - instance where estimation will be stored.
      Returns:
      true if result estimation was updated, false otherwise.

    • getState

      public GNSSKalmanState getState()
      Gets current Kalman filter state containing current GNSS estimation along with Kalman filter covariance error matrix.
      Returns:
      current Kalman filter state containing current GNSS estimation along with Kalman filter covariance error matrix.

    • getState

      public boolean getState(GNSSKalmanState result)
      Gets current Kalman filter state containing current GNSS estimation along with Kalman filter covariance error matrix. This method does not update result instance if no state is available.
      Parameters:
      result - instance where state will be stored.
      Returns:
      true if result state was updated, false otherwise.

    • getLastStateTimestamp

      public Double getLastStateTimestamp()
      Gets timestamp expressed in seconds since epoch time when Kalman filter state was last propagated.
      Returns:
      timestamp expressed in seconds since epoch time when Kalman filter state was last propagated.

    • getLastStateTimestampAsTime

      public boolean getLastStateTimestampAsTime(com.irurueta.units.Time result)
      Gets timestamp since epoch time when Kalman filter state was last propagated.
      Parameters:
      result - instance where timestamp since epoch time when Kalman filter state was last propagated will be stored.
      Returns:
      true if result instance is updated, false otherwise.

    • getLastStateTimestampAsTime

      public com.irurueta.units.Time getLastStateTimestampAsTime()
      Gets timestamp since epoch time when Kalman filter state was last propagated.
      Returns:
      timestamp since epoch time when Kalman filter state was last propagated.

    • isRunning

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

    • isUpdateMeasurementsReady

      public static boolean isUpdateMeasurementsReady(Collection<GNSSMeasurement> measurements)
      Indicates whether provided measurements are ready to be used for an update.
      Parameters:
      measurements - measurements to be checked.
      Returns:
      true if estimator is ready, false otherwise.

    • updateMeasurements

      public boolean updateMeasurements(Collection<GNSSMeasurement> measurements, com.irurueta.units.Time timestamp) throws LockedException, NotReadyException, GNSSException
      Updates GNSS measurements of this estimator when new satellite measurements are available. Calls to this method will be ignored if interval between provided timestamp and last timestamp when Kalman filter was updated is less than epoch interval.
      Parameters:
      measurements - GNSS measurements to be updated.
      timestamp - timestamp since epoch time when GNSS measurements were updated.
      Returns:
      true if measurements were updated, false otherwise.
      Throws:
      LockedException - if this estimator is already running.
      NotReadyException - if estimator is not ready for measurements updates.
      GNSSException - if estimation fails due to numerical instabilities.

    • updateMeasurements

      public boolean updateMeasurements(Collection<GNSSMeasurement> measurements, double timestamp) throws LockedException, NotReadyException, GNSSException
      Updates GNSS measurements of this estimator when new satellite measurements are available. Call to this method will be ignored if interval between provided timestamp and last timestamp when Kalman filter was updated is less than epoch interval.
      Parameters:
      measurements - GNSS measurements to be updated.
      timestamp - timestamp expressed in seconds since epoch time when GNSS measurements were updated.
      Returns:
      true if measurements were updated, false otherwise.
      Throws:
      LockedException - if this estimator is already running.
      NotReadyException - if estimator is not ready for measurements updates.
      GNSSException - if estimation fails due to numerical instabilities.

    • isPropagateReady

      public boolean isPropagateReady()
      Indicates whether this estimator is ready for state propagations.
      Returns:
      true if estimator is ready, false otherwise.

    • propagate

      public boolean propagate(com.irurueta.units.Time timestamp) throws LockedException, NotReadyException, GNSSException
      Propagates Kalman filter state held by this estimator at provided timestamp. Call to this method will be ignored if interval between provided timestamp and last timestamp when Kalman filter was updated is less than epoch interval.
      Parameters:
      timestamp - timestamp since epoch to propagate state.
      Returns:
      true if state was propagated, false otherwise.
      Throws:
      LockedException - if this estimator is already running.
      NotReadyException - if estimator is not ready for measurements updates.
      GNSSException - if estimation fails due to numerical instabilities.

    • propagate

      public boolean propagate(double timestamp) throws LockedException, NotReadyException, GNSSException
      Propagates Kalman filter state held by this estimator at provided timestamp. Call to this method will be ignored if interval between provided timestamp and last timestamp when Kalman filter was updated is less than epoch interval.
      Parameters:
      timestamp - timestamp expressed in seconds since epoch to propagate state.
      Returns:
      true if state was propagated, false otherwise.
      Throws:
      LockedException - if this estimator is already running.
      NotReadyException - if estimator is not ready for measurements updates.
      GNSSException - if estimation fails due to numerical instabilities.

    • reset

      public void reset() throws LockedException
      Resets this estimator.
      Throws:
      LockedException - if this estimator is already running.