Package com.irurueta.navigation.inertial

Class INSLooselyCoupledKalmanFilteredEstimator

java.lang.Object
com.irurueta.navigation.inertial.INSLooselyCoupledKalmanFilteredEstimator
public class INSLooselyCoupledKalmanFilteredEstimator extends Object
Calculates position, velocity, attitude and IMU biases using an INS loosely coupled Kalman filter to take into account inertial measurements to smooth results and taking into account an initial position. This implementation is based on the equations defined in "Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems, Second Edition" and on the companion software available at: https://github.com/ymjdz/MATLAB-Codes/blob/master/Loosely_coupled_INS_GNSS.m

  • Field Details

    • listener

      private INSLooselyCoupledKalmanFilteredEstimatorListener 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 less than this value, will be ignored.

    • config

      private INSLooselyCoupledKalmanConfig config
      INS loosely coupled Kalman filter configuration parameters (usually obtained through calibration).

    • kinematics

      private BodyKinematics kinematics
      Last provided user kinematics containing applied specific force and angular rates resolved in body axes.

    • correctedKinematics

      private BodyKinematics correctedKinematics
      Contains last provided user kinematics minus currently estimated bias for acceleration and angular rate values.

    • frame

      private com.irurueta.navigation.frames.ECEFFrame frame
      Contains current or initial user position, velocity and attitude.

    • initialConfig

      private INSLooselyCoupledKalmanInitializerConfig initialConfig
      Configuration containing uncertainty measures to set initial covariance matrix within estimated state. Once this estimator is initialized, covariance will be updated with new provided INS measurements until convergence is reached.

    • state

      private INSLooselyCoupledKalmanState state
      Current Kalman filter state containing current INS 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

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator()
      Constructor.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanConfig config)
      Constructor.
      Parameters:
      config - INS Kalman filter configuration parameters (usually obtained through calibration).

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(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.

    • INSLooselyCoupledKalmanFilteredEstimator

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

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanConfig config, double epochInterval)
      Constructor.
      Parameters:
      config - INS loosely coupled 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.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanConfig config, INSLooselyCoupledKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      config - INS tightly coupled Kalman filter configuration parameters (usually obtained through calibration).
      listener - listener to notify events raised by this instance.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(double epochInterval, INSLooselyCoupledKalmanFilteredEstimatorListener 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.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanConfig config, double epochInterval, INSLooselyCoupledKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      config - INS loosely coupled 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.

    • INSLooselyCoupledKalmanFilteredEstimator

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

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanConfig config, com.irurueta.units.Time epochInterval)
      Constructor.
      Parameters:
      config - INS tightly coupled 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.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(com.irurueta.units.Time epochInterval, INSLooselyCoupledKalmanFilteredEstimatorListener 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.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanConfig config, com.irurueta.units.Time epochInterval, INSLooselyCoupledKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      config - INS loosely coupled 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.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(com.irurueta.navigation.frames.ECEFFrame frame)
      Constructor.
      Parameters:
      frame - frame containing initial user position, velocity and attitude resolved along ECEF axes.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanConfig config, com.irurueta.navigation.frames.ECEFFrame frame)
      Constructor.
      Parameters:
      config - INS Kalman filter configuration parameters (usually obtained through calibration).
      frame - frame containing initial user position, velocity and attitude resolved along ECEF axes.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(double epochInterval, com.irurueta.navigation.frames.ECEFFrame frame)
      Constructor.
      Parameters:
      epochInterval - minimum epoch interval expressed in seconds (s) between consecutive propagations or measurements.
      frame - frame containing initial user position, velocity and attitude resolved along ECEF axes.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(com.irurueta.navigation.frames.ECEFFrame frame, INSLooselyCoupledKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      frame - frame containing initial user position, velocity and attitude resolved along ECEF axes.
      listener - listener to notify events raised by this instance.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanConfig config, double epochInterval, com.irurueta.navigation.frames.ECEFFrame frame)
      Constructor.
      Parameters:
      config - INS loosely coupled Kalman filter configuration parameters (usually obtained through calibration).
      epochInterval - minimum epoch interval expressed in seconds (s) between consecutive propagations or measurements.
      frame - frame containing initial user position, velocity and attitude resolved along ECEF axes.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanConfig config, com.irurueta.navigation.frames.ECEFFrame frame, INSLooselyCoupledKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      config - INS loosely coupled Kalman filter configuration parameters (usually obtained through calibration).
      frame - frame containing initial user position, velocity and attitude resolved along ECEF axes.
      listener - listener to notify events raised by this instance.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(double epochInterval, com.irurueta.navigation.frames.ECEFFrame frame, INSLooselyCoupledKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      epochInterval - minimum epoch interval expressed in seconds (s) between consecutive propagations or measurements.
      frame - frame containing initial user position, velocity and attitude resolved along ECEF axes.
      listener - listener to notify events raised by this instance.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanConfig config, double epochInterval, com.irurueta.navigation.frames.ECEFFrame frame, INSLooselyCoupledKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      config - INS loosely coupled Kalman filter configuration parameters (usually obtained through calibration).
      epochInterval - minimum epoch interval expressed in seconds (s) between consecutive propagations or measurements.
      frame - frame containing initial user position, velocity and attitude resolved along ECEF axes.
      listener - listener to notify events raised by this instance.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(com.irurueta.units.Time epochInterval, com.irurueta.navigation.frames.ECEFFrame frame)
      Constructor.
      Parameters:
      epochInterval - minimum epoch interval between consecutive propagations or measurements.
      frame - frame containing initial user position, velocity and attitude resolved along eCEF axes.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanConfig config, com.irurueta.units.Time epochInterval, com.irurueta.navigation.frames.ECEFFrame frame)
      Constructor.
      Parameters:
      config - INS loosely coupled Kalman filter configuration parameters (usually obtained through calibration).
      epochInterval - minimum epoch interval between consecutive propagations or measurements.
      frame - frame containing initial user position, velocity and attitude resolved along ECEF axes.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(com.irurueta.units.Time epochInterval, com.irurueta.navigation.frames.ECEFFrame frame, INSLooselyCoupledKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      epochInterval - minimum epoch interval between consecutive propagations or measurements.
      frame - frame containing initial user position, velocity and attitude resolved along ECEF axes.
      listener - listener to notify events raised by this instance.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanConfig config, com.irurueta.units.Time epochInterval, com.irurueta.navigation.frames.ECEFFrame frame, INSLooselyCoupledKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      config - INS loosely coupled Kalman filter configuration parameters (usually obtained through calibration).
      epochInterval - minimum epoch interval between consecutive propagations or measurements.
      frame - frame containing initial user position, velocity and attitude resolved along ECEF axes.
      listener - listener to notify events raised by this instance.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanInitializerConfig initialConfig)
      Constructor.
      Parameters:
      initialConfig - initial INS loosely coupled Kalman configuration to set proper initial covariance during filter initialization.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanConfig config, INSLooselyCoupledKalmanInitializerConfig initialConfig)
      Constructor.
      Parameters:
      config - INS Kalman filter configuration parameters (usually obtained through calibration).
      initialConfig - initial INS loosely coupled Kalman configuration to set proper initial covariance during filter initialization.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(double epochInterval, INSLooselyCoupledKalmanInitializerConfig initialConfig)
      Constructor.
      Parameters:
      epochInterval - minimum epoch interval expressed in seconds (s) between consecutive propagations or measurements.
      initialConfig - initial INS loosely coupled Kalman configuration to set proper initial covariance during filter initialization.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanInitializerConfig initialConfig, INSLooselyCoupledKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      initialConfig - initial INS loosely coupled Kalman configuration to set proper initial covariance during filter initialization.
      listener - listener to notify events raised by this instance.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanConfig config, double epochInterval, INSLooselyCoupledKalmanInitializerConfig initialConfig)
      Constructor.
      Parameters:
      config - INS loosely coupled Kalman filter configuration parameters (usually obtained through calibration).
      epochInterval - minimum epoch interval expressed in seconds (s) between consecutive propagations or measurements.
      initialConfig - initial INS loosely coupled Kalman configuration to set proper initial covariance during filter initialization.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanConfig config, INSLooselyCoupledKalmanInitializerConfig initialConfig, INSLooselyCoupledKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      config - INS loosely coupled Kalman filter configuration parameters (usually obtained through calibration).
      initialConfig - initial INS loosely coupled Kalman configuration to set proper initial covariance during filter initialization.
      listener - listener to notify events raised by this instance.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(double epochInterval, INSLooselyCoupledKalmanInitializerConfig initialConfig, INSLooselyCoupledKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      epochInterval - minimum epoch interval expressed in seconds (s) between consecutive propagations or measurements.
      initialConfig - initial INS loosely coupled Kalman configuration to set proper initial covariance during filter initialization.
      listener - listener to notify events raised by this instance.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanConfig config, double epochInterval, INSLooselyCoupledKalmanInitializerConfig initialConfig, INSLooselyCoupledKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      config - INS loosely coupled Kalman filter configuration parameters (usually obtained through calibration).
      epochInterval - minimum epoch interval expressed in seconds (s) between consecutive propagations or measurements.
      initialConfig - initial INS loosely coupled Kalman configuration to set proper initial covariance during filter initialization.
      listener - listener to notify events raised by this instance.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(com.irurueta.units.Time epochInterval, INSLooselyCoupledKalmanInitializerConfig initialConfig)
      Constructor.
      Parameters:
      epochInterval - minimum epoch interval between consecutive propagations or measurements.
      initialConfig - initial INS loosely coupled Kalman configuration to set proper initial covariance during filter initialization.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanConfig config, com.irurueta.units.Time epochInterval, INSLooselyCoupledKalmanInitializerConfig initialConfig)
      Constructor.
      Parameters:
      config - INS loosely coupled Kalman filter configuration parameters (usually obtained through calibration).
      epochInterval - minimum epoch interval between consecutive propagations or measurements.
      initialConfig - initial INS loosely coupled Kalman configuration to set proper initial covariance during filter initialization.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(com.irurueta.units.Time epochInterval, INSLooselyCoupledKalmanInitializerConfig initialConfig, INSLooselyCoupledKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      epochInterval - minimum epoch interval between consecutive propagations or measurements.
      initialConfig - initial INS loosely coupled Kalman configuration to set proper initial covariance during filter initialization.
      listener - listener to notify events raised by this instance.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanConfig config, com.irurueta.units.Time epochInterval, INSLooselyCoupledKalmanInitializerConfig initialConfig, INSLooselyCoupledKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      config - INS loosely coupled Kalman filter configuration parameters (usually obtained through calibration).
      epochInterval - minimum epoch interval between consecutive propagations or measurements.
      initialConfig - initial INS loosely coupled Kalman configuration to set proper initial covariance during filter initialization.
      listener - listener to notify events raised by this instance.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanInitializerConfig initialConfig, com.irurueta.navigation.frames.ECEFFrame frame)
      Constructor.
      Parameters:
      initialConfig - initial INS loosely coupled Kalman configuration to set proper initial covariance during filter initialization.
      frame - frame containing initial user position, velocity and attitude resolved along ECEF axes.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanConfig config, INSLooselyCoupledKalmanInitializerConfig initialConfig, com.irurueta.navigation.frames.ECEFFrame frame)
      Constructor.
      Parameters:
      config - INS Kalman filter configuration parameters (usually obtained through calibration).
      initialConfig - initial INS loosely coupled Kalman configuration to set proper initial covariance during filter initialization.
      frame - frame containing initial user position, velocity and attitude resolved along ECEF axes.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(double epochInterval, INSLooselyCoupledKalmanInitializerConfig initialConfig, com.irurueta.navigation.frames.ECEFFrame frame)
      Constructor.
      Parameters:
      epochInterval - minimum epoch interval expressed in seconds (s) between consecutive propagations or measurements.
      initialConfig - initial INS loosely coupled Kalman configuration to set proper initial covariance during filter initialization.
      frame - frame containing initial user position, velocity and attitude resolved along ECEF axes.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanInitializerConfig initialConfig, com.irurueta.navigation.frames.ECEFFrame frame, INSLooselyCoupledKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      initialConfig - initial INS loosely coupled Kalman configuration to set proper initial covariance during filter initialization.
      frame - frame containing initial user position, velocity and attitude resolved along ECEF axes.
      listener - listener to notify events raised by this instance.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanConfig config, double epochInterval, INSLooselyCoupledKalmanInitializerConfig initialConfig, com.irurueta.navigation.frames.ECEFFrame frame)
      Constructor.
      Parameters:
      config - INS loosely coupled Kalman filter configuration parameters (usually obtained through calibration).
      epochInterval - minimum epoch interval expressed in seconds (s) between consecutive propagations or measurements.
      initialConfig - initial INS loosely coupled Kalman configuration to set proper initial covariance during filter initialization.
      frame - frame containing initial user position, velocity and attitude resolved along ECEF axes.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanConfig config, INSLooselyCoupledKalmanInitializerConfig initialConfig, com.irurueta.navigation.frames.ECEFFrame frame, INSLooselyCoupledKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      config - INS loosely coupled Kalman filter configuration parameters (usually obtained through calibration).
      initialConfig - initial INS loosely coupled Kalman configuration to set proper initial covariance during filter initialization.
      frame - frame containing initial user position, velocity and attitude resolved along ECEF axes.
      listener - listener to notify events raised by this instance.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(double epochInterval, INSLooselyCoupledKalmanInitializerConfig initialConfig, com.irurueta.navigation.frames.ECEFFrame frame, INSLooselyCoupledKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      epochInterval - minimum epoch interval expressed in seconds (s) between consecutive propagations or measurements.
      initialConfig - initial INS loosely coupled Kalman configuration to set proper initial covariance during filter initialization.
      frame - frame containing initial user position, velocity and attitude resolved along ECEF axes.
      listener - listener to notify events raised by this instance.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanConfig config, double epochInterval, INSLooselyCoupledKalmanInitializerConfig initialConfig, com.irurueta.navigation.frames.ECEFFrame frame, INSLooselyCoupledKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      config - INS loosely coupled Kalman filter configuration parameters (usually obtained through calibration).
      epochInterval - minimum epoch interval expressed in seconds (s) between consecutive propagations or measurements.
      initialConfig - initial INS loosely coupled Kalman configuration to set proper initial covariance during filter initialization.
      frame - frame containing initial user position, velocity and attitude resolved along ECEF axes.
      listener - listener to notify events raised by this instance.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(com.irurueta.units.Time epochInterval, INSLooselyCoupledKalmanInitializerConfig initialConfig, com.irurueta.navigation.frames.ECEFFrame frame)
      Constructor.
      Parameters:
      epochInterval - minimum epoch interval between consecutive propagations or measurements.
      initialConfig - initial INS loosely coupled Kalman configuration to set proper initial covariance during filter initialization.
      frame - frame containing initial user position, velocity and attitude resolved along ECEF axes.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanConfig config, com.irurueta.units.Time epochInterval, INSLooselyCoupledKalmanInitializerConfig initialConfig, com.irurueta.navigation.frames.ECEFFrame frame)
      Constructor.
      Parameters:
      config - INS loosely coupled Kalman filter configuration parameters (usually obtained through calibration).
      epochInterval - minimum epoch interval between consecutive propagations or measurements.
      initialConfig - initial INS loosely coupled Kalman configuration to set proper initial covariance during filter initialization.
      frame - frame containing initial user position, velocity and attitude resolved along ECEF axes.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(com.irurueta.units.Time epochInterval, INSLooselyCoupledKalmanInitializerConfig initialConfig, com.irurueta.navigation.frames.ECEFFrame frame, INSLooselyCoupledKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      epochInterval - minimum epoch interval between consecutive propagations or measurements.
      initialConfig - initial INS loosely coupled Kalman configuration to set proper initial covariance during filter initialization.
      frame - frame containing initial user position, velocity and attitude resolved along ECEF axes.
      listener - listener to notify events raised by this instance.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

    • INSLooselyCoupledKalmanFilteredEstimator

      public INSLooselyCoupledKalmanFilteredEstimator(INSLooselyCoupledKalmanConfig config, com.irurueta.units.Time epochInterval, INSLooselyCoupledKalmanInitializerConfig initialConfig, com.irurueta.navigation.frames.ECEFFrame frame, INSLooselyCoupledKalmanFilteredEstimatorListener listener)
      Constructor.
      Parameters:
      config - INS loosely coupled Kalman filter configuration parameters (usually obtained through calibration).
      epochInterval - minimum epoch interval between consecutive propagations or measurements.
      initialConfig - initial INS loosely coupled Kalman configuration to set proper initial covariance during filter initialization.
      frame - frame containing initial user position, velocity and attitude resolved along ECEF axes.
      listener - listener to notify events raised by this instance.
      Throws:
      IllegalArgumentException - if provided epoch interval is negative.

  • Method Details

    • getListener

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

    • setListener

      public void setListener(INSLooselyCoupledKalmanFilteredEstimatorListener listener) throws com.irurueta.navigation.LockedException
      Sets listener to notify events raised by this instance.
      Parameters:
      listener - listener to notify events raised by this instance.
      Throws:
      com.irurueta.navigation.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 com.irurueta.navigation.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 interval are less than this value, will be ignored.
      Parameters:
      epochInterval - minimum epoch interval expressed in seconds (s) between consecutive propagations or measurements.
      Throws:
      com.irurueta.navigation.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 com.irurueta.navigation.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:
      com.irurueta.navigation.LockedException - if this estimator is already running.
      IllegalArgumentException - if provided epoch interval is negative.

    • getConfig

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

    • getConfig

      public INSLooselyCoupledKalmanConfig getConfig()
      Gets INS loosely coupled Kalman configuration parameters (usually obtained through calibration).
      Returns:
      INS loosely coupled Kalman configuration parameters.

    • setConfig

      public void setConfig(INSLooselyCoupledKalmanConfig config) throws com.irurueta.navigation.LockedException
      Sets INS loosely coupled Kalman configuration parameters (usually obtained through calibration).
      Parameters:
      config - INS loosely coupled Kalman configuration parameters to be set.
      Throws:
      com.irurueta.navigation.LockedException - if this estimator is already running.

    • getFrame

      public boolean getFrame(com.irurueta.navigation.frames.ECEFFrame result)
      Gets ECEF frame containing current or initial user position, velocity and attitude.
      Parameters:
      result - instance where current ECEF frame will be stored.
      Returns:
      true if provided result instance is updated, false otherwise.

    • getFrame

      public com.irurueta.navigation.frames.ECEFFrame getFrame()
      Gets ECEF frame containing current or initial user position, velocity and attitude.
      Returns:
      ECEF frame containing current or initial user position, velocity and attitude.

    • setFrame

      public void setFrame(com.irurueta.navigation.frames.ECEFFrame frame) throws com.irurueta.navigation.LockedException
      Sets ECEF frame containing current or initial user position, velocity and attitude.
      Parameters:
      frame - ECEF frame containing current or initial user position, velocity and attitude to be set.
      Throws:
      com.irurueta.navigation.LockedException - if this estimator is already running.

    • getInitialConfig

      public boolean getInitialConfig(INSLooselyCoupledKalmanInitializerConfig result)
      Gets initial INS loosely coupled Kalman configuration to set a proper initial covariance matrix during the first Kalman filter propagation. Once this estimator is initialized, covariance will be updated with new provided INS measurements until convergence is reached.
      Parameters:
      result - instance where configuration data will be stored.
      Returns:
      true if result instance was updated, false otherwise.

    • getInitialConfig

      public INSLooselyCoupledKalmanInitializerConfig getInitialConfig()
      Gets initial INS loosely coupled Kalman configuration to set a proper initial covariance matrix during the first Kalman filter propagation. Once this estimator is initialized, covariance will be updated with new provided INS measurements until convergence is reached.
      Returns:
      initial INS loosely coupled Kalman configuration.

    • setInitialConfig

      public void setInitialConfig(INSLooselyCoupledKalmanInitializerConfig initialConfig) throws com.irurueta.navigation.LockedException
      Sets initial INS loosely coupled Kalman configuration to set a proper initial covariance matrix during the first Kalman filter propagation. Once this estimator is initialized, covariance will be updated with new provided INS measurements until convergence is reached.
      Parameters:
      initialConfig - initial configuration to be set.
      Throws:
      com.irurueta.navigation.LockedException - if this estimator is already running.

    • getKinematics

      public BodyKinematics getKinematics()
      Gets last provided user kinematics containing applied specific force and angular rates resolved in body axes.
      Returns:
      last provided user kinematics.

    • getKinematics

      public boolean getKinematics(BodyKinematics result)
      Gets last provided user kinematics containing applied specific force and angular rates resolved in body axes.
      Parameters:
      result - instance where last provided body kinematics will be stored.
      Returns:
      true if provided result instance was updated, false otherwise.

    • getCorrectedKinematics

      public BodyKinematics getCorrectedKinematics()
      Gets corrected kinematics which are the last provided user kinematics after removal of the biases estimated by the Kalman filter.
      Returns:
      corrected kinematics.
      See Also:

    • getCorrectedKinematics

      public boolean getCorrectedKinematics(BodyKinematics result)
      Gets corrected kinematics which are the las provided user kinematics after removal of the biases estimated by the Kalman filter.
      Parameters:
      result - instance where corrected body kinematics will be stored.
      Returns:
      true if provided result instance was updated, false otherwise.

    • getState

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

    • getState

      public boolean getState(INSLooselyCoupledKalmanState result)
      Gets current Kalman filter state containing current INS 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.

    • isUpdateReady

      public boolean isUpdateReady()
      Indicates whether this instance is ready to update state using available IMU data (specific force and angular rates).
      Returns:
      true if ready, false otherwise.

    • update

      public boolean update(BodyKinematics kinematics, com.irurueta.units.Time timestamp) throws com.irurueta.navigation.LockedException, com.irurueta.navigation.NotReadyException, INSException
      Updates specific force and angular rate applied to the user's body expressed in coordinates resolved along body-frame axes.
      Parameters:
      kinematics - kinematics applied to body (specific force and angular rate) during las period of time. These measures are obtained from an inertial unit (IMU).
      timestamp - timestamp since epoch time when specific force and angular rate values were updated.
      Returns:
      true if body kinematics values were updated, false otherwise.
      Throws:
      com.irurueta.navigation.LockedException - if this estimator is already running.
      com.irurueta.navigation.NotReadyException - if this estimator is not ready to be updated.
      INSException - if estimation fails due to numerical instabilities.

    • update

      public boolean update(BodyKinematics kinematics, double timestamp) throws com.irurueta.navigation.LockedException, com.irurueta.navigation.NotReadyException, INSException
      Updates specific force and angular rate applied to the user's body expressed in coordinates resolved along body-frame axes.
      Parameters:
      kinematics - kinematics applied to body (specific force and angular rate) during las period of time. These measures are obtained from an inertial unit (IMU).
      timestamp - timestamp expressed in seconds since epoch time when specific force and angular rate values were updated.
      Returns:
      true if body kinematics values were updated, false otherwise.
      Throws:
      com.irurueta.navigation.LockedException - if this estimator is already running.
      com.irurueta.navigation.NotReadyException - if this estimator is not ready to be updated.
      INSException - 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 com.irurueta.navigation.LockedException, com.irurueta.navigation.NotReadyException, INSException
      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:
      com.irurueta.navigation.LockedException - if this estimator is already running.
      com.irurueta.navigation.NotReadyException - if estimator is not ready for measurements updates.
      INSException - if estimation fails due to numerical instabilities.

    • propagate

      public boolean propagate(double timestamp) throws com.irurueta.navigation.LockedException, com.irurueta.navigation.NotReadyException, INSException
      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:
      com.irurueta.navigation.LockedException - if this estimator is already running.
      com.irurueta.navigation.NotReadyException - if estimator is not ready for measurements updates.
      INSException - if estimation fails due to numerical instabilities.

    • reset

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

    • initInitialConfig

      private void initInitialConfig()
      Initializes initial INS loosely coupled Kalman configuration to set a proper initial covariance matrix. This method makes no action if an initial configuration already exists.

    • correctKinematics

      private void correctKinematics(BodyKinematics kinematics)
      Corrects provided kinematics by taking into account currently estimated specific force and angular rate biases. This method stores the result into the variable member containing corrected kinematics values.
      Parameters:
      kinematics - kinematics instance to be corrected.

    • getValueOrZero

      private double getValueOrZero(double value)
      Returns provided value if not infinity and not NaN.
      Parameters:
      value - value to be returned.
      Returns:
      value or 0.0.