Package com.irurueta.navigation.lateration

Class NonLinearLeastSquaresLaterationSolver<P extends com.irurueta.geometry.Point<?>>

java.lang.Object
com.irurueta.navigation.lateration.LaterationSolver<P>
com.irurueta.navigation.lateration.NonLinearLeastSquaresLaterationSolver<P>
Type Parameters:
P - a Point type.
Direct Known Subclasses:
NonLinearLeastSquaresLateration2DSolver, NonLinearLeastSquaresLateration3DSolver
public abstract class NonLinearLeastSquaresLaterationSolver<P extends com.irurueta.geometry.Point<?>> extends LaterationSolver<P>
Solves a Trilateration problem with an instance of the least squares optimizer. By solving the lateration problem linearly, this class is able to estimate the covariance of estimated position. To achieve better results, it is usually better to provide an initial coarse solution. This class is base on the implementation found at: https://github.com/lemmingapex/trilateration

  • Field Details

    • DEFAULT_DISTANCE_STANDARD_DEVIATION

      public static final double DEFAULT_DISTANCE_STANDARD_DEVIATION
      Default standard deviation assumed for provided distances when none is explicitly provided.
      See Also:

    • fitter

      private final com.irurueta.numerical.fitting.LevenbergMarquardtMultiDimensionFitter fitter
      Levenberg-Marquardt fitter to find a non-linear solution.

    • covariance

      private com.irurueta.algebra.Matrix covariance
      Estimated covariance matrix for estimated position.

    • chiSq

      private double chiSq
      Estimated chi square value.

    • initialPosition

      private P extends com.irurueta.geometry.Point<?> initialPosition
      Initial position to start lateration solving. If not defined, centroid of provided position points will be used.

    • distanceStandardDeviations

      private double[] distanceStandardDeviations
      Standard deviations of provided distances.

  • Constructor Details

    • NonLinearLeastSquaresLaterationSolver

      protected NonLinearLeastSquaresLaterationSolver()
      Constructor.

    • NonLinearLeastSquaresLaterationSolver

      protected NonLinearLeastSquaresLaterationSolver(P[] positions, double[] distances)
      Constructor.
      Parameters:
      positions - known positions of static nodes.
      distances - euclidean distances from static nodes to mobile node.
      Throws:
      IllegalArgumentException - if either positions or distances are null, don't have the same length or their length is smaller than required 2 points.

    • NonLinearLeastSquaresLaterationSolver

      protected NonLinearLeastSquaresLaterationSolver(P initialPosition)
      Constructor.
      Parameters:
      initialPosition - initial position to start lateration solving.

    • NonLinearLeastSquaresLaterationSolver

      protected NonLinearLeastSquaresLaterationSolver(P[] positions, double[] distances, P initialPosition)
      Constructor.
      Parameters:
      positions - known positions of static nodes.
      distances - euclidean distances from static nodes to mobile node.
      initialPosition - initial position to start lateration solving.
      Throws:
      IllegalArgumentException - if either positions or distances are null, don't have the same length or their length is smaller than required (3 for 2D points or 4 for 3D points) or fitter is null.

    • NonLinearLeastSquaresLaterationSolver

      protected NonLinearLeastSquaresLaterationSolver(LaterationSolverListener<P> listener)
      Constructor.
      Parameters:
      listener - listener to be notified of events raised by this instance.

    • NonLinearLeastSquaresLaterationSolver

      protected NonLinearLeastSquaresLaterationSolver(P[] positions, double[] distances, LaterationSolverListener<P> listener)
      Constructor.
      Parameters:
      positions - known positions of static nodes.
      distances - euclidean distances from static nodes to mobile node.
      listener - listener to be notified of events raised by this instance.
      Throws:
      IllegalArgumentException - if either positions or distances are null, don't have the same length or their length is smaller than required 2 points.

    • NonLinearLeastSquaresLaterationSolver

      protected NonLinearLeastSquaresLaterationSolver(P initialPosition, LaterationSolverListener<P> listener)
      Constructor.
      Parameters:
      initialPosition - initial position to start lateration solving.
      listener - listener to be notified of events raised by this instance.

    • NonLinearLeastSquaresLaterationSolver

      protected NonLinearLeastSquaresLaterationSolver(P[] positions, double[] distances, P initialPosition, LaterationSolverListener<P> listener)
      Constructor.
      Parameters:
      positions - known positions of static nodes.
      distances - euclidean distances from static nodes to mobile node.
      initialPosition - initial position to start lateration solving.
      listener - listener to be notified of events raised by this instance.
      Throws:
      IllegalArgumentException - if either positions or distances are null, don't have the same length or their length is smaller than required (3 for 2D points or 4 for 3D points) or fitter is null.

    • NonLinearLeastSquaresLaterationSolver

      protected NonLinearLeastSquaresLaterationSolver(P[] positions, double[] distances, double[] distanceStandardDeviations)
      Constructor.
      Parameters:
      positions - known positions of static nodes.
      distances - euclidean distances from static nodes to mobile node.
      distanceStandardDeviations - standard deviations of provided measured distances.
      Throws:
      IllegalArgumentException - if either positions, distances or standard deviations are null, don't have the same length of their length is smaller than required (2 points).

    • NonLinearLeastSquaresLaterationSolver

      protected NonLinearLeastSquaresLaterationSolver(P[] positions, double[] distances, double[] distanceStandardDeviations, P initialPosition)
      Constructor.
      Parameters:
      positions - known positions of static nodes.
      distances - euclidean distances from static nodes to mobile node.
      distanceStandardDeviations - standard deviations of provided measured distances.
      initialPosition - initial position to start lateration solving.
      Throws:
      IllegalArgumentException - if either positions, distances or standard deviations are null, don't have the same length of their length is smaller than required (2 points).

    • NonLinearLeastSquaresLaterationSolver

      protected NonLinearLeastSquaresLaterationSolver(P[] positions, double[] distances, double[] distanceStandardDeviations, LaterationSolverListener<P> listener)
      Constructor.
      Parameters:
      positions - known positions of static nodes.
      distances - euclidean distances from static nodes to mobile node.
      distanceStandardDeviations - standard deviations of provided measured distances.
      listener - listener to be notified of events raised by this instance.
      Throws:
      IllegalArgumentException - if either positions, distances or standard deviations are null, don't have the same length of their length is smaller than required (2 points).

    • NonLinearLeastSquaresLaterationSolver

      protected NonLinearLeastSquaresLaterationSolver(P[] positions, double[] distances, double[] distanceStandardDeviations, P initialPosition, LaterationSolverListener<P> listener)
      Constructor.
      Parameters:
      positions - known positions of static nodes.
      distances - euclidean distances from static nodes to mobile node.
      distanceStandardDeviations - standard deviations of provided measured distances.
      initialPosition - initial position to start lateration solving.
      listener - listener to be notified of events raised by this instance.
      Throws:
      IllegalArgumentException - if either positions, distances or standard deviations are null, don't have the same length of their length is smaller than required (2 points).

  • Method Details

    • setPositionsAndDistances

      public void setPositionsAndDistances(P[] positions, double[] distances) throws LockedException
      Sets known positions and Euclidean distances. If any distance value is zero or negative, it will be fixed assuming an EPSILON value.
      Overrides:
      setPositionsAndDistances in class LaterationSolver<P extends com.irurueta.geometry.Point<?>>
      Parameters:
      positions - known positions of static nodes.
      distances - euclidean distances from static nodes to mobile node.
      Throws:
      IllegalArgumentException - if either positions or distances are null, don't have the same length or their length is smaller than required (2 points).
      LockedException - if instance is busy solving the lateration problem.

    • setPositionsDistancesAndStandardDeviations

      public void setPositionsDistancesAndStandardDeviations(P[] positions, double[] distances, double[] distanceStandardDeviations) throws LockedException
      Sets known positions, Euclidean distances and the respective standard deviations of measured distances. If any distance value is zero or negative, it will be fixed assuming an EPSILON value.
      Parameters:
      positions - known positions of static nodes.
      distances - euclidean distances from static nodes to mobile node.
      distanceStandardDeviations - standard deviations of provided measured distances.
      Throws:
      IllegalArgumentException - if either positions, distances or standard deviations are null, don't have the same length of their length is smaller than required (2 points).
      LockedException - if instance is busy solving the trilateration problem.

    • getDistanceStandardDeviations

      public double[] getDistanceStandardDeviations()
      Gets standard deviations of provided distances.
      Returns:
      standard deviations of provided distances.

    • getCovariance

      public com.irurueta.algebra.Matrix getCovariance()
      Gets estimated covariance matrix for estimated position.
      Returns:
      estimated covariance matrix for estimated position.

    • getChiSq

      public double getChiSq()
      Gets estimated chi square value.
      Returns:
      estimated chi square value.

    • getInitialPosition

      public P getInitialPosition()
      Gets initial position to start lateration solving. If not defined, centroid of provided position points will be used to start lateration.
      Returns:
      initial position to start lateration.

    • setInitialPosition

      public void setInitialPosition(P initialPosition) throws LockedException
      Sets initial position to start lateration solving. If not defined, centroid of provided position points will be used to start lateration.
      Parameters:
      initialPosition - initial position to start lateration.
      Throws:
      LockedException - if instance is busy solving the lateration problem.

    • solve

      public void solve() throws LaterationException, NotReadyException, LockedException
      Solves the lateration problem.
      Specified by:
      solve in class LaterationSolver<P extends com.irurueta.geometry.Point<?>>
      Throws:
      LaterationException - if lateration fails.
      NotReadyException - is solver is not ready.
      LockedException - if instance is busy solving the lateration problem.

    • getType

      public LaterationSolverType getType()
      Gets lateration solver type.
      Specified by:
      getType in class LaterationSolver<P extends com.irurueta.geometry.Point<?>>
      Returns:
      lateration solver type.

    • internalSetPositionsAndDistances

      protected void internalSetPositionsAndDistances(P[] positions, double[] distances)
      Internally sets known positions and Euclidean distances. If any distance value is zero or negative, it will be fixed assuming an EPSILON value.
      Overrides:
      internalSetPositionsAndDistances in class LaterationSolver<P extends com.irurueta.geometry.Point<?>>
      Parameters:
      positions - known positions of static nodes.
      distances - euclidean distances from static nodes to mobile node.
      Throws:
      IllegalArgumentException - if either positions or distances are null, don't have the same length or their length is smaller than required (2 points).

    • internalSetPositionsDistancesAndStandardDeviations

      protected void internalSetPositionsDistancesAndStandardDeviations(P[] positions, double[] distances, double[] distanceStandardDeviations)
      Internally sets known positions, Euclidean distances and the respective standard deviations of measured distances. If any distance value is zero or negative, it will be fixed assuming an EPSILON value.
      Parameters:
      positions - known positions of static nodes.
      distances - euclidean distances from static nodes to mobile node.
      distanceStandardDeviations - standard deviations of provided measured distances.
      Throws:
      IllegalArgumentException - if either positions, distances or standard deviations are null, don't have the same length of their length is smaller than required (2 points).

    • setupFitter

      private void setupFitter() throws com.irurueta.numerical.fitting.FittingException
      Setups fitter to solve lateration problem.
      Throws:
      com.irurueta.numerical.fitting.FittingException - if Levenberg-Marquardt fitting fails.