public HomogeneousLinearLeastSquaresLateration2DSolver(
            final Circle[] circles, final LaterationSolverListener<Point2D> listener) {
        super(listener);
        internalSetCircles(circles);
    }

    /**
     * Gets circles defined by provided positions and distances.
     *
     * @return circles defined by provided positions and distances.
     */
    public Circle[] getCircles() {
        if (positions == null) {
            return null;
        }

        final var result = new Circle[positions.length];

        for (var i = 0; i < positions.length; i++) {
            result[i] = new Circle(positions[i], distances[i]);
        }
        return result;
    }

    /**
     * Sets circles defining positions and euclidean distances.
     *
     * @param circles circles defining positions and distances.
     * @throws IllegalArgumentException if circles is null or length of array of circles
     *                                  is less than 2.
     * @throws LockedException          if instance is busy solving the lateration problem.
     */
    public void setCircles(final Circle[] circles) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetCircles(circles);
    }

    /**
     * Gets number of dimensions of provided points.
     *
     * @return always returns 2 dimensions.
     */
    @Override
    public int getNumberOfDimensions() {
        return Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH;
    }

    /**
     * Minimum required number of positions and distances.
     * At least 3 positions and distances will be required to linearly solve a 2D problem.
     *
     * @return minimum required number of positions and distances.
     */
    @Override
    public int getMinRequiredPositionsAndDistances() {
        return Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH + 1;
    }

    /**
     * Gets estimated position.
     *
     * @return estimated position.
     */
    @Override
    public Point2D getEstimatedPosition() {
        if (estimatedPositionCoordinates == null) {
            return null;
        }

        final var position = new InhomogeneousPoint2D();
        getEstimatedPosition(position);
        return position;
    }

    /**
     * Internally sets circles defining positions and Euclidean distances.
     *
     * @param circles circles defining positions and distances.
     * @throws IllegalArgumentException if circles is null or length of array of circles
     *                                  is less than 3.
     */
    private void internalSetCircles(final Circle[] circles) {
        if (circles == null || circles.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        final var positions = new Point2D[circles.length];
        final var distances = new double[circles.length];
        for (var i = 0; i < circles.length; i++) {
            final var circle = circles[i];
            positions[i] = circle.getCenter();
            distances[i] = circle.getRadius();
        }

        internalSetPositionsAndDistances(positions, distances);
    }
}

public HomogeneousLinearLeastSquaresLateration3DSolver(
            final Sphere[] spheres, final LaterationSolverListener<Point3D> listener) {
        super(listener);
        internalSetSpheres(spheres);
    }

    /**
     * Gets spheres defined by provided positions and distances.
     *
     * @return spheres defined by provided positions and distances.
     */
    public Sphere[] getSpheres() {
        if (positions == null) {
            return null;
        }

        final var result = new Sphere[positions.length];

        for (var i = 0; i < positions.length; i++) {
            result[i] = new Sphere(positions[i], distances[i]);
        }
        return result;
    }

    /**
     * Sets spheres defining positions and Euclidean distances.
     *
     * @param spheres spheres defining positions and distances.
     * @throws IllegalArgumentException if spheres is null or length of array of spheres
     *                                  is less than 2.
     * @throws LockedException          if instance is busy solving the lateration problem.
     */
    public void setSpheres(final Sphere[] spheres) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetSpheres(spheres);
    }

    /**
     * Gets number of dimensions of provided points.
     *
     * @return always returns 2 dimensions.
     */
    @Override
    public int getNumberOfDimensions() {
        return Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH;
    }

    /**
     * Minimum required number of positions and distances.
     * At least 4 positions and distances will be required to linearly solve a 3D problem.
     *
     * @return minimum required number of positions and distances.
     */
    @Override
    public int getMinRequiredPositionsAndDistances() {
        return Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH + 1;
    }

    /**
     * Gets estimated position.
     *
     * @return estimated position.
     */
    @Override
    public Point3D getEstimatedPosition() {
        if (estimatedPositionCoordinates == null) {
            return null;
        }

        final var position = new InhomogeneousPoint3D();
        getEstimatedPosition(position);
        return position;
    }

    /**
     * Internally sets spheres defining positions and Euclidean distances.
     *
     * @param spheres spheres defining positions and distances.
     * @throws IllegalArgumentException if spheres is null or length of array of spheres
     *                                  is less than 4.
     */
    private void internalSetSpheres(final Sphere[] spheres) {
        if (spheres == null || spheres.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        final var positions = new Point3D[spheres.length];
        final var distances = new double[spheres.length];
        for (var i = 0; i < spheres.length; i++) {
            final var sphere = spheres[i];
            positions[i] = sphere.getCenter();
            distances[i] = sphere.getRadius();
        }

        internalSetPositionsAndDistances(positions, distances);
    }
}

}

    /**
     * Internally sets circles defining positions and Euclidean distances.
     *
     * @param circles circles defining positions and distances.
     * @throws IllegalArgumentException if circles is null or length of array of circles
     *                                  is less than 2.
     */
    private void internalSetCircles(final Circle[] circles) {
        if (circles == null || circles.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        final var positions = new Point2D[circles.length];
        final var distances = new double[circles.length];
        for (var i = 0; i < circles.length; i++) {
            final var circle = circles[i];
            positions[i] = circle.getCenter();
            distances[i] = circle.getRadius();
        }

        internalSetPositionsAndDistances(positions, distances);
    }

    /**
     * Internally sets circles defining positions and Euclidean distances along with the standard
     * deviations of provided circles radii.
     *
     * @param circles                  circles defining positions and distances.
     * @param radiusStandardDeviations standard deviations of circles radii.
     * @throws IllegalArgumentException if circles is null, length of arrays is less than
     *                                  2 or don't have the same length.
     */
    private void internalSetCirclesAndStandardDeviations(
            final Circle[] circles, final double[] radiusStandardDeviations) {
        if (circles == null || circles.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        if (radiusStandardDeviations == null) {
            throw new IllegalArgumentException();
        }

        if (radiusStandardDeviations.length != circles.length) {
            throw new IllegalArgumentException();
        }

        final var positions = new Point2D[circles.length];
        final var distances = new double[circles.length];
        for (var i = 0; i < circles.length; i++) {
            final var circle = circles[i];
            positions[i] = circle.getCenter();
            distances[i] = circle.getRadius();
        }

        internalSetPositionsDistancesAndStandardDeviations(positions, distances, radiusStandardDeviations);
    }

public void internalSetSpheres(final Sphere[] spheres) {
        if (spheres == null || spheres.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        final var positions = new Point3D[spheres.length];
        final var distances = new double[spheres.length];
        for (var i = 0; i < spheres.length; i++) {
            final var sphere = spheres[i];
            positions[i] = sphere.getCenter();
            distances[i] = sphere.getRadius();
        }

        internalSetPositionsAndDistances(positions, distances);
    }

    /**
     * Internally sets spheres defining positions and Euclidean distances along with the standard
     * deviations of provided spheres radii.
     *
     * @param spheres                  spheres defining positions and distances.
     * @param radiusStandardDeviations standard deviations of circles radii.
     * @throws IllegalArgumentException if spheres is null, length of arrays is less than
     *                                  2 or don't have the same length.
     */
    private void internalSetSpheresAndStandardDeviations(
            final Sphere[] spheres, final double[] radiusStandardDeviations) {
        if (spheres == null || spheres.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        if (radiusStandardDeviations == null) {
            throw new IllegalArgumentException();
        }

        if (radiusStandardDeviations.length != spheres.length) {
            throw new IllegalArgumentException();
        }

        final var positions = new Point3D[spheres.length];
        final var distances = new double[spheres.length];
        for (var i = 0; i < spheres.length; i++) {
            final var sphere = spheres[i];
            positions[i] = sphere.getCenter();
            distances[i] = sphere.getRadius();
        }

        internalSetPositionsDistancesAndStandardDeviations(positions, distances, radiusStandardDeviations);
    }

return new Speed(getVelocityNorm(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets coordinate of velocity of body frame with respect ECEF frame and
     * resolved along North axis.
     *
     * @param result instance where North velocity coordinate will be stored.
     */
    public void getSpeedN(final Speed result) {
        result.setValue(vn);
        result.setUnit(SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets coordinate of velocity of body frame with respect ECEF frame and
     * resolved along North axis.
     *
     * @return North velocity coordinate.
     */
    public Speed getSpeedN() {
        return new Speed(vn, SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets coordinate of velocity of body frame with respect ECEF frame and
     * resolved along North axis.
     *
     * @param speedN North velocity coordinate to be set.
     */
    public void setSpeedN(final Speed speedN) {
        vn = SpeedConverter.convert(speedN.getValue().doubleValue(), speedN.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets coordinate of velocity of body frame with respect ECEF frame and
     * resolved along East axis.
     *
     * @param result instance where East velocity coordinate will be stored.
     */
    public void getSpeedE(final Speed result) {
        result.setValue(ve);
        result.setUnit(SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets coordinate of velocity of body frame with respect ECEF frame and
     * resolved along East axis.
     *
     * @return East velocity coordinate.
     */
    public Speed getSpeedE() {
        return new Speed(ve, SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets coordinate of velocity of body frame with respect ECEF frame and
     * resolved along East axis.
     *
     * @param speedE East velocity coordinate to be set.
     */
    public void setSpeedE(final Speed speedE) {
        ve = SpeedConverter.convert(speedE.getValue().doubleValue(), speedE.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets coordinate of velocity of body frame with respect ECEF frame and
     * resolved along Down axis.
     *
     * @param result instance where Down velocity coordinate will be stored.
     */
    public void getSpeedD(final Speed result) {
        result.setValue(vd);
        result.setUnit(SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets coordinate of velocity of body frame with respect ECEF frame and
     * resolved along Down axis.
     *
     * @return Down velocity coordinate.
     */
    public Speed getSpeedD() {
        return new Speed(vd, SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets coordinate of velocity of body frame with respect ECEF frame and
     * resolved along Down axis.
     *
     * @param speedD Down velocity coordinate to be set.
     */
    public void setSpeedD(final Speed speedD) {
        vd = SpeedConverter.convert(speedD.getValue().doubleValue(), speedD.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets velocity coordinates of body frame resolved along North, East, Down
     * axes.
     *
     * @param speedN North velocity coordinate.
     * @param speedE East velocity coordinate.
     * @param speedD Down velocity coordinate.
     */
    public void setSpeedCoordinates(final Speed speedN, final Speed speedE, final Speed speedD) {

return new Speed(getVelocityNorm(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets x coordinate of velocity of body frame resolved along ECEF-frame axes.
     *
     * @param result instance where x coordinate of velocity will be stored.
     */
    public void getSpeedX(final Speed result) {
        result.setValue(vx);
        result.setUnit(SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets x coordinate of velocity of body frame resolved along ECI or ECEF-frame axes.
     *
     * @return x coordinate of velocity of body frame resolved along ECI or ECEF-frame axes.
     */
    public Speed getSpeedX() {
        return new Speed(vx, SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets x coordinate of velocity of body frame resolved along ECI or ECEF-frame axes.
     *
     * @param speedX x coordinate of velocity of body frame resolved along ECI or ECEF-frame
     *               axes to be set.
     */
    public void setSpeedX(final Speed speedX) {
        vx = SpeedConverter.convert(speedX.getValue().doubleValue(), speedX.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets y coordinate of velocity of body frame resolved along ECI or ECEF-frame axes.
     *
     * @param result instance where y coordinate of velocity will be stored.
     */
    public void getSpeedY(final Speed result) {
        result.setValue(vy);
        result.setUnit(SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets y coordinate of velocity of body frame resolved along ECI or ECEF-frame axes.
     *
     * @return y coordinate of velocity of body frame resolved along ECI or ECEF-frame axes.
     */
    public Speed getSpeedY() {
        return new Speed(vy, SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets y coordinate of velocity of body frame resolved along ECI or ECEF-frame axes.
     *
     * @param speedY y coordinate of velocity of body frame resolved along ECI or ECEF-frame
     *               axes to be set.
     */
    public void setSpeedY(final Speed speedY) {
        vy = SpeedConverter.convert(speedY.getValue().doubleValue(), speedY.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets z coordinate of velocity of body frame resolved along ECI or ECEF-frame axes.
     *
     * @param result instance where z coordinate of velocity will be stored.
     */
    public void getSpeedZ(final Speed result) {
        result.setValue(vz);
        result.setUnit(SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets z coordinate of velocity of body frame resolved along ECI or ECEF-frame axes.
     *
     * @return z coordinate of velocity of body frame resolved along ECI or ECEF-frame axes.
     */
    public Speed getSpeedZ() {
        return new Speed(vz, SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets z coordinate of velocity of body frame resolved along ECI or ECEF-frame axes.
     *
     * @param speedZ z coordinate of velocity of body frame resolved along ECI or ECEF-frame
     *               axes to be set.
     */
    public void setSpeedZ(final Speed speedZ) {
        vz = SpeedConverter.convert(speedZ.getValue().doubleValue(), speedZ.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets velocity coordinates of body frame resolved along ECI or ECEF-frame axes.
     *
     * @param speedX x coordinate of velocity to be set.
     * @param speedY y coordinate of velocity to be set.
     * @param speedZ z coordinate of velocity to be set.
     */
    public void setSpeedCoordinates(final Speed speedX, final Speed speedY, final Speed speedZ) {

super(circles, distanceStandardDeviations, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this solver is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if robust solver is locked because an
     *                                  estimation is already in progress.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == distances.length;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Solves the lateration problem.
     *
     * @return estimated position.
     * @throws LockedException          if instance is busy solving the lateration problem.
     * @throws NotReadyException        is solver is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public Point2D solve() throws LockedException, NotReadyException, RobustEstimatorException {

final var inlierPositions = new Point2D[nInliers];
            final var inlierDistances = new double[nInliers];
            double[] inlierStandardDeviations = null;
            if (distanceStandardDeviations != null) {
                inlierStandardDeviations = new double[nInliers];
            }
            var pos = 0;
            for (var i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    // sample is inlier
                    inlierPositions[pos] = positions[i];
                    inlierDistances[pos] = distances[i];
                    if (inlierStandardDeviations != null) {
                        inlierStandardDeviations[pos] = distanceStandardDeviations[i];
                    }
                    pos++;
                }
            }

            try {
                nonLinearSolver.setInitialPosition(position);
                if (inlierStandardDeviations != null) {
                    nonLinearSolver.setPositionsDistancesAndStandardDeviations(inlierPositions, inlierDistances,
                            inlierStandardDeviations);
                } else {
                    nonLinearSolver.setPositionsAndDistances(inlierPositions, inlierDistances);
                }
                nonLinearSolver.solve();

                if (keepCovariance) {
                    // keep covariance
                    covariance = nonLinearSolver.getCovariance();
                } else {
                    covariance = null;
                }

                estimatedPosition = nonLinearSolver.getEstimatedPosition();
            } catch (Exception e) {
                // refinement failed, so we return input value
                covariance = null;
                estimatedPosition = position;
            }
        } else {
            covariance = null;
            estimatedPosition = position;
        }

        return estimatedPosition;
    }

    /**
     * Solves a preliminary solution for a subset of samples picked by a robust estimator.
     *
     * @param samplesIndices indices of samples picked by the robust estimator.
     * @param solutions      list where estimated preliminary solution will be stored.
     */
    protected void solvePreliminarySolutions(final int[] samplesIndices, final List<Point2D> solutions) {

listener.onSolveProgressChange(PROSACRobustLateration2DSolver.this, progress);
                }
            }
        });

        try {
            locked = true;

            if (listener != null) {
                listener.onSolveStart(this);
            }

            inliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            var result = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();
            result = attemptRefine(result);

            if (listener != null) {
                listener.onSolveEnd(this);
            }

            return result;

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            locked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 3 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null || qualityScores.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        this.qualityScores = qualityScores;
    }
}

listener.onSolveProgressChange(PROMedSRobustLateration2DSolver.this, progress);
                }
            }
        });

        try {
            locked = true;

            if (listener != null) {
                listener.onSolveStart(this);
            }

            inliersData = null;
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            innerEstimator.setUseInlierThresholds(false);
            var result = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();
            result = attemptRefine(result);

            if (listener != null) {
                listener.onSolveEnd(this);
            }

            return result;

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            locked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMEDS;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 3 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null || qualityScores.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        this.qualityScores = qualityScores;
    }
}

copyFrom(input);
    }

    /**
     * Gets cartesian x coordinate of body position expressed in meters (m) with respect
     * ECEF frame, resolved along ECEF axes.
     *
     * @return cartesian x coordinate of body position.
     */
    public double getX() {
        return x;
    }

    /**
     * Sets cartesian x coordinate of body position expressed in meters (m) with respect
     * ECEF frame, resolved along ECEF axes.
     *
     * @param x cartesian x coordinate of body position.
     */
    public void setX(final double x) {
        this.x = x;
    }

    /**
     * Gets cartesian y coordinate of body position expressed in meters (m) with respect
     * ECEF frame, resolved along ECEF axes.
     *
     * @return cartesian y coordinate of body position.
     */
    public double getY() {
        return y;
    }

    /**
     * Sets cartesian y coordinate of body position expressed in meters (m) with respect
     * ECEF frame, resolved along ECEF axes.
     *
     * @param y cartesian y coordinate of body position.
     */
    public void setY(final double y) {
        this.y = y;
    }

    /**
     * Gets cartesian z coordinate of body position expressed in meters (m) with respect
     * ECEF frame, resolved along ECEF axes.
     *
     * @return z coordinate of body position.
     */
    public double getZ() {
        return z;
    }

    /**
     * Sets cartesian z coordinate of body position expressed in meters (m) with respect
     * ECEF frame, resolved along ECEF axes.
     *
     * @param z cartesian z coordinate of body position.
     */
    public void setZ(final double z) {
        this.z = z;
    }

    /**
     * Sets cartesian coordinates of body position expressed in meters (m) and resolved
     * along ECEF-frame axes.
     *
     * @param x cartesian x coordinate of body position.
     * @param y cartesian y coordinate of body position.
     * @param z cartesian z coordinate of body position.
     */
    public void setCoordinates(final double x, final double y, final double z) {
        this.x = x;
        this.y = y;
        this.z = z;
    }

    /**
     * Gets body position expressed in meters (m) and resolved along ECEF-frame axes.
     *
     * @return body position.
     */
    public Point3D getPosition() {
        return new InhomogeneousPoint3D(x, y, z);
    }

    /**
     * Gets body position expressed in meters (m) and resolved along ECEF-frame axes.
     *
     * @param result instance where position data is copied to.
     */
    public void getPosition(final Point3D result) {
        result.setInhomogeneousCoordinates(x, y, z);
    }

    /**
     * Sets body position expressed in meters (m) and resolved along ECEF-frame axes.
     *
     * @param point body position to be set.
     */
    public void setPosition(final Point3D point) {
        x = point.getInhomX();
        y = point.getInhomY();
        z = point.getInhomZ();
    }

    /**
     * Gets cartesian x coordinate of body position resolved along ECEF-frame axes.
     *
     * @param result instance where cartesian x coordinate of body position will be
     *               stored.
     */
    public void getDistanceX(final Distance result) {

internalSetCircles(circles);
    }

    /**
     * Gets number of dimensions of provided points.
     *
     * @return always returns 2 dimensions.
     */
    @Override
    public int getNumberOfDimensions() {
        return Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH;
    }

    /**
     * Minimum required number of positions and distances.
     * At least 3 positions and distances will be required to linearly solve a 2D problem.
     *
     * @return minimum required number of positions and distances.
     */
    @Override
    public int getMinRequiredPositionsAndDistances() {
        return Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH + 1;
    }

    /**
     * Gets estimated position.
     *
     * @return estimated position.
     */
    @Override
    public Point2D getEstimatedPosition() {
        if (estimatedPositionCoordinates == null) {
            return null;
        }

        final var position = new InhomogeneousPoint2D();
        getEstimatedPosition(position);
        return position;
    }

    /**
     * Internally sets circles defining positions and Euclidean distances.
     *
     * @param circles circles defining positions and distances.
     * @throws IllegalArgumentException if circles is null or length of array of circles
     *                                  is less than 3.
     */
    private void internalSetCircles(final Circle[] circles) {
        if (circles == null || circles.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        final var positions = new Point2D[circles.length];
        final var distances = new double[circles.length];
        for (var i = 0; i < circles.length; i++) {
            final var circle = circles[i];
            positions[i] = circle.getCenter();
            distances[i] = circle.getRadius();
        }

        internalSetPositionsAndDistances(positions, distances);
    }

internalSetCircles(circles);
    }

    /**
     * Gets number of dimensions of provided points.
     *
     * @return always returns 2 dimensions.
     */
    @Override
    public int getNumberOfDimensions() {
        return Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH;
    }

    /**
     * Minimum required number of positions and distances.
     * At least 3 positions and distances will be required to linearly solve a 2D problem.
     *
     * @return minimum required number of positions and distances.
     */
    @Override
    public int getMinRequiredPositionsAndDistances() {
        return Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH + 1;
    }

    /**
     * Gets estimated position.
     *
     * @return estimated position.
     */
    @Override
    public Point2D getEstimatedPosition() {
        if (estimatedPositionCoordinates == null) {
            return null;
        }

        final var position = new InhomogeneousPoint2D();
        getEstimatedPosition(position);
        return position;
    }

    /**
     * Internally sets circles defining positions and Euclidean distances.
     *
     * @param circles circles defining positions and distances.
     * @throws IllegalArgumentException if circles is null or length of array of circles
     *                                  is less than 3.
     */
    private void internalSetCircles(final Circle[] circles) {
        if (circles == null || circles.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        final var positions = new Point2D[circles.length];
        final var distances = new double[circles.length];
        for (var i = 0; i < circles.length; i++) {
            final var circle = circles[i];
            positions[i] = circle.getCenter();
            distances[i] = circle.getRadius();
        }

        internalSetPositionsAndDistances(positions, distances);
    }

setSpeedZ(speedZ);
    }

    /**
     * Gets coordinate transformation matrix.
     * This is equivalent to calling getCoordinateTransformation(), but more efficient
     *
     * @return coordinate transformation matrix.
     */
    @Override
    public Matrix getCoordinateTransformationMatrix() {
        Matrix result;
        try {
            result = new Matrix(CoordinateTransformation.ROWS, CoordinateTransformation.COLS);
            getCoordinateTransformationMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets coordinate transformation matrix.
     * This is equivalent to calling getCoordinateTransformation().getMatrix(), but more efficient
     *
     * @param result instance where coordinate transformation matrix will be copied to.
     */
    @Override
    public void getCoordinateTransformationMatrix(final Matrix result) {
        c.matrix.copyTo(result);
    }

    /**
     * Sets coordinate transformation matrix keeping current source and destination {@link FrameType}.
     * This is more efficient than getting a copy of coordinate transformation calling to
     * {@link #getCoordinateTransformation()}, setting coordinate matrix into copied coordinate transformation and
     * then setting the coordinate transformation calling {@link #setCoordinateTransformation(CoordinateTransformation)}.
     *
     * @param matrix    a 3x3 coordinate transformation matrix to be set.
     * @param threshold threshold to validate rotation matrix.
     * @throws InvalidRotationMatrixException if provided matrix is not a valid rotation matrix (3x3 and orthonormal).
     * @throws IllegalArgumentException       if provided threshold is negative.
     */
    @Override
    public void setCoordinateTransformationMatrix(final Matrix matrix, final double threshold)
            throws InvalidRotationMatrixException {
        c.setMatrix(matrix,threshold);
    }

    /**
     * Sts coordinate transformation matrix keeping current source and destination {@link FrameType}.
     * This is more efficient than getting a copy of coordinate transformation calling to
     * {@link #getCoordinateTransformation()}, setting coordinate matrix into copied coordinate transformation and
     * then setting the coordinate transformation calling {@link #setCoordinateTransformation(CoordinateTransformation)}.
     *
     * @param matrix a 3x3 coordinate transformation matrix to be set.
     * @throws InvalidRotationMatrixException if provided matrix is not a valid rotation matrix (3x3 and orthonormal).
     */
    @Override
    public void setCoordinateTransformationMatrix(final Matrix matrix) throws InvalidRotationMatrixException {
        c.setMatrix(matrix);
    }

    /**
     * Gets coordinate transformation as a new 3D rotation instance.
     * This is equivalent to calling getCoordinateTransformation().asRotation(), but more efficient.
     *
     * @return new coordinate transformation as a 3D rotation.
     * @throws InvalidRotationMatrixException if internal matrix cannot be converted to a 3D rotation.
     */
    @Override
    public Rotation3D getCoordinateTransformationRotation() throws InvalidRotationMatrixException {
        return c.asRotation();
    }

    /**
     * Gets coordinate transformation as a 3D rotation.
     * This is equivalent to calling getCoordinateTransformation().asRotation(), but more efficient.
     *
     * @param result instance where coordinate transformation 3D rotation will be copied to.
     * @throws InvalidRotationMatrixException if internal matrix cannot be converted to a 3D rotation.
     */
    @Override
    public void getCoordinateTransformationRotation(final Rotation3D result) throws InvalidRotationMatrixException {
        c.asRotation(result);
    }

    /**
     * Sets coordinate transformation from 3D rotation and keeping current source and destination {@link FrameType}.
     * This is more efficient than getting a copy of coordinate transformation calling to
     * {@link #getCoordinateTransformation()}, setting rotation into copied coordinate transformation and
     * then setting the coordinate transformation calling {@link #setCoordinateTransformation(CoordinateTransformation)}.
     *
     * @param rotation set rotation into current coordinate rotation.
     */
    @Override
    public void setCoordinateTransformationRotation(final Rotation3D rotation) {
        c.fromRotation(rotation);
    }

listener.onSolveProgressChange(PROSACRobustLateration2DSolver.this, progress);
                }
            }
        });

        try {
            locked = true;

            if (listener != null) {
                listener.onSolveStart(this);
            }

            inliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            var result = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();
            result = attemptRefine(result);

            if (listener != null) {
                listener.onSolveEnd(this);
            }

            return result;

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            locked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;

listener.onSolveProgressChange(LMedSRobustLateration2DSolver.this, progress);
                }
            }
        });

        try {
            locked = true;

            if (listener != null) {
                listener.onSolveStart(this);
            }

            inliersData = null;
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            var result = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();
            result = attemptRefine(result);

            if (listener != null) {
                listener.onSolveEnd(this);
            }

            return result;

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            locked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMEDS;
    }
}

SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets estimated ECEF user position expressed in meters (m).
     *
     * @param result instance where estimated ECEF user position will be stored.
     */
    public void getPosition(final Point3D result) {
        result.setInhomogeneousCoordinates(x, y, z);
    }

    /**
     * Gets estimated ECEF user position expressed in meters (m).
     *
     * @return estimated ECEF user position.
     */
    public Point3D getPosition() {
        return new InhomogeneousPoint3D(x, y, z);
    }

    /**
     * Sets estimated ECEF user position expressed in meters (m).
     *
     * @param position estimated ECEF user position.
     */
    public void setPosition(final Point3D position) {
        x = position.getInhomX();
        y = position.getInhomY();
        z = position.getInhomZ();
    }

    /**
     * Gets estimatedECEF user position.
     *
     * @param result instance where result will be stored.
     */
    public void getEcefPosition(final ECEFPosition result) {
        result.setCoordinates(x, y, z);
    }

    /**
     * Gets estimated ECEF user position.
     *
     * @return estimated ECEF user position.
     */
    public ECEFPosition getEcefPosition() {
        return new ECEFPosition(x, y, z);
    }

    /**
     * Sets estimated ECEF user position.
     *
     * @param ecefPosition estimated ECEF user position.
     */
    public void setEcefPosition(final ECEFPosition ecefPosition) {
        x = ecefPosition.getX();
        y = ecefPosition.getY();
        z = ecefPosition.getZ();
    }

    /**
     * Gets estimated ECEF user velocity.
     *
     * @param result instance where result will be stored.
     */
    public void getEcefVelocity(final ECEFVelocity result) {

super(circles, distanceStandardDeviations, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on distance between estimated position and distances
     * provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if this solver is locked.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Solves the lateration problem.
     *
     * @return estimated position.
     * @throws LockedException          if instance is busy solving the lateration problem.
     * @throws NotReadyException        is solver is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public Point2D solve() throws LockedException, NotReadyException, RobustEstimatorException {

super(circles, distanceStandardDeviations, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if this solver is locked.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each pair of
     * positions and distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each pair of positions and
     * distances (i.e. sample).
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if robust solver is locked because an
     *                                  estimation is already in progress.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == distances.length;
    }

    /**
     * Solves the lateration problem.
     *
     * @return estimated position.
     * @throws LockedException          if instance is busy solving the lateration problem.
     * @throws NotReadyException        is solver is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public Point2D solve() throws LockedException, NotReadyException, RobustEstimatorException {

}

    /**
     * Gets circles defined by provided positions and distances.
     *
     * @return circles defined by provided positions and distances.
     */
    public Circle[] getCircles() {
        if (positions == null) {
            return null;
        }

        final var result = new Circle[positions.length];

        for (var i = 0; i < positions.length; i++) {
            result[i] = new Circle(positions[i], distances[i]);
        }
        return result;
    }

    /**
     * Sets circles defining positions and euclidean distances.
     *
     * @param circles circles defining positions and distances.
     * @throws IllegalArgumentException if circles is null or length of array of circles
     *                                  is less than 2.
     * @throws LockedException          if instance is busy solving the lateration problem.
     */
    public void setCircles(final Circle[] circles) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetCircles(circles);
    }

    /**
     * Sets circles defining positions and Euclidean distances along with the standard
     * deviations of provided circles radii.
     *
     * @param circles                  circles defining positions and distances.
     * @param radiusStandardDeviations standard deviations of circles radii.
     * @throws IllegalArgumentException if circles is null, length of arrays is less than
     *                                  2 or don't have the same length.
     * @throws LockedException          if instance is busy solving the lateration problem.
     */
    public void setCirclesAndStandardDeviations(final Circle[] circles, final double[] radiusStandardDeviations)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetCirclesAndStandardDeviations(circles, radiusStandardDeviations);
    }

}

    /**
     * Gets spheres defined by provided positions and distances.
     *
     * @return spheres defined by provided positions and distances.
     */
    public Sphere[] getSpheres() {
        if (positions == null) {
            return null;
        }

        final var result = new Sphere[positions.length];

        for (var i = 0; i < positions.length; i++) {
            result[i] = new Sphere(positions[i], distances[i]);
        }
        return result;
    }

    /**
     * Sets spheres defining positions and Euclidean distances.
     *
     * @param spheres spheres defining positions and distances.
     * @throws IllegalArgumentException if spheres is null or length of array of spheres
     *                                  is less than 2.
     * @throws LockedException          if instance is busy solving the lateration problem.
     */
    public void setSpheres(final Sphere[] spheres) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetSpheres(spheres);
    }

    /**
     * Sets spheres defining positions and Euclidean distances along with the standard
     * deviations of provided spheres radii.
     *
     * @param spheres                  spheres defining positions and distances.
     * @param radiusStandardDeviations standard deviations of circles radii.
     * @throws IllegalArgumentException if spheres is null, length of arrays is less than
     *                                  2 or don't have the same length.
     * @throws LockedException          if instance is busy solving the lateration problem.
     */
    public void setSpheresAndStandardDeviations(final Sphere[] spheres, final double[] radiusStandardDeviations)
            throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        internalSetSpheresAndStandardDeviations(spheres, radiusStandardDeviations);
    }

var estimatedPosition = initialPosition;
            if (useLinearSolver) {
                if (useHomogeneousLinearSolver) {
                    homogeneousLinearSolver.setPositionsAndDistances(innerPositions, innerDistances);
                    homogeneousLinearSolver.solve();
                    estimatedPosition = homogeneousLinearSolver.getEstimatedPosition();
                } else {
                    inhomogeneousLinearSolver.setPositionsAndDistances(innerPositions, innerDistances);
                    inhomogeneousLinearSolver.solve();
                    estimatedPosition = inhomogeneousLinearSolver.getEstimatedPosition();
                }
            }

            if (refinePreliminarySolutions || estimatedPosition == null) {
                nonLinearSolver.setInitialPosition(estimatedPosition);
                if (distanceStandardDeviations != null) {
                    nonLinearSolver.setPositionsDistancesAndStandardDeviations(innerPositions,
                            innerDistances, innerDistanceStandardDeviations);
                } else {
                    nonLinearSolver.setPositionsAndDistances(innerPositions, innerDistances);
                }
                nonLinearSolver.solve();
                estimatedPosition = nonLinearSolver.getEstimatedPosition();
            }

            solutions.add(estimatedPosition);
        } catch (final NavigationException ignore) {
            // if anything fails, no solution is added
        }
    }

    /**
     * Internally sets circles defining positions and Euclidean distances.
     *
     * @param circles circles defining positions and distances.
     * @throws IllegalArgumentException if circles is null or length of array of circles
     *                                  is less than {@link #getMinRequiredPositionsAndDistances}.
     */
    private void internalSetCircles(final Circle[] circles) {

z = ecefPosition.getZ();
    }

    /**
     * Gets estimated ECEF user velocity.
     *
     * @param result instance where result will be stored.
     */
    public void getEcefVelocity(final ECEFVelocity result) {
        result.setCoordinates(vx, vy, vz);
    }

    /**
     * Gets estimated ECEF user velocity.
     *
     * @return estimated ECEF user velocity.
     */
    public ECEFVelocity getEcefVelocity() {
        return new ECEFVelocity(vx, vy, vz);
    }

    /**
     * Sets estimated ECEF user velocity.
     *
     * @param ecefVelocity estimated ECEF user velocity.
     */
    public void setEcefVelocity(final ECEFVelocity ecefVelocity) {
        vx = ecefVelocity.getVx();
        vy = ecefVelocity.getVy();
        vz = ecefVelocity.getVz();
    }

    /**
     * Gets estimated ECEF user position and velocity.
     *
     * @param result instance where result will be stored.
     */
    public void getPositionAndVelocity(final ECEFPositionAndVelocity result) {
        result.setPositionCoordinates(x, y, z);
        result.setVelocityCoordinates(vx, vy, vz);
    }

    /**
     * Gets estimated ECEF user position and velocity.
     *
     * @return estimated ECEF user position and velocity.
     */
    public ECEFPositionAndVelocity getPositionAndVelocity() {
        return new ECEFPositionAndVelocity(x, y, z, vx, vy, vz);
    }

    /**
     * Sets estimated ECEF user position and velocity.
     *
     * @param positionAndVelocity estimated ECEF user position and velocity.
     */
    public void setPositionAndVelocity(final ECEFPositionAndVelocity positionAndVelocity) {

}

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Solves the lateration problem.
     *
     * @return estimated position.
     * @throws LockedException          if instance is busy solving the lateration problem.
     * @throws NotReadyException        is solver is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public Point2D solve() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<Point2D>() {

}

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if this solver is locked.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Solves the lateration problem.
     *
     * @return estimated position.
     * @throws LockedException          if instance is busy solving the lateration problem.
     * @throws NotReadyException        is solver is not ready.
     * @throws RobustEstimatorException if estimation fails for any reason
     *                                  (i.e. numerical instability, no solution available, etc).
     */
    @Override
    public Point3D solve() throws LockedException, NotReadyException, RobustEstimatorException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<Point3D>() {

public abstract int getMinRequiredPositionsAndDistances();

    /**
     * Internally sets known positions and Euclidean distances.
     * If any distance value is zero or negative, it will be fixed assuming an EPSILON value.
     *
     * @param positions known positions of static nodes.
     * @param 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).
     */
    protected void internalSetPositionsAndDistances(final P[] positions, final double[] distances) {
        if (positions == null || distances == null) {
            throw new IllegalArgumentException();
        }

        if (positions.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        if (positions.length != distances.length) {
            throw new IllegalArgumentException();
        }

        this.positions = positions;
        this.distances = distances;

        // fix distances if needed
        for (var i = 0; i < this.distances.length; i++) {
            if (this.distances[i] < EPSILON) {
                this.distances[i] = EPSILON;
            }
        }
    }

public ECEFPositionAndVelocity(final ECEFPositionAndVelocity input) {
        copyFrom(input);
    }

    /**
     * Gets cartesian x coordinate of position resolved in ECEF axes and
     * expressed in meters (m).
     *
     * @return cartesian x coordinate of position resolved in ECEF axes
     * and expressed in meters (m).
     */
    public double getX() {
        return x;
    }

    /**
     * Sets cartesian x coordinate of position resolved in ECEF axes and
     * expressed in meters (m).
     *
     * @param x cartesian x coordinate of position resolved in ECEF axes
     *          and expressed in meters (m).
     */
    public void setX(final double x) {
        this.x = x;
    }

    /**
     * Gets cartesian y coordinate of position resolved in ECEF axes and
     * expressed in meters (m).
     *
     * @return cartesian y coordinate of position resolved in ECEF axes
     * and expressed in meters (m).
     */
    public double getY() {
        return y;
    }

    /**
     * Sets cartesian y coordinate of position resolved in ECEF axes and
     * expressed in meters (m).
     *
     * @param y cartesian y coordinate of position resolved in ECEF axes
     *          and expressed in meters (m).
     */
    public void setY(final double y) {
        this.y = y;
    }

    /**
     * Gets cartesian z coordinate of position resolved in ECEF axes and
     * expressed in meters (m).
     *
     * @return cartesian z coordinate of position resolved in ECEF axes
     * and expressed in meters (m).
     */
    public double getZ() {
        return z;
    }

    /**
     * Sets cartesian z coordinate of position resolved in ECEF axes and
     * expressed in meters (m).
     *
     * @param z cartesian z coordinate of position resolved in ECEF axes
     *          and expressed in meters (m).
     */
    public void setZ(final double z) {
        this.z = z;
    }

    /**
     * Sets ECEF position expressed in meters (m).
     *
     * @param x cartesian x coordinate of position.
     * @param y cartesian y coordinate of position.
     * @param z cartesian z coordinate of position.
     */
    public void setPositionCoordinates(final double x, final double y, final double z) {
        this.x = x;
        this.y = y;
        this.z = z;
    }

    /**
     * Gets cartesian x coordinate of position resolved in ECEF axes.
     *
     * @param result instance where cartesian x coordinate of position will
     *               be stored.
     */
    public void getXDistance(final Distance result) {

innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            var result = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();
            result = attemptRefine(result);

            if (listener != null) {
                listener.onSolveEnd(this);
            }

            return result;

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            locked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMEDS;

innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            var result = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();
            result = attemptRefine(result);

            if (listener != null) {
                listener.onSolveEnd(this);
            }

            return result;

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            locked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMEDS;

innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            var result = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();
            result = attemptRefine(result);

            if (listener != null) {
                listener.onSolveEnd(this);
            }

            return result;

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            locked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.MSAC;

innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            var result = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();
            result = attemptRefine(result);

            if (listener != null) {
                listener.onSolveEnd(this);
            }

            return result;

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } finally {
            locked = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.MSAC;

if ((tcrossings & 1) != 0) {
            tempsum += (tempsum < 0 ? 1 : -1) * area0 / 2;
        }

        // area is with the clockwise sense. If !reverse convert to counter-clockwise convention
        if (!reverse) {
            tempsum *= -1;
        }

        // if sign put area in (-area0/2, area0/2], else put area in [0, area0)
        if (sign) {
            if (tempsum > area0 / 2) {
                tempsum -= area0;
            } else if (tempsum <= -area0 / 2) {
                tempsum += area0;
            }
        } else {
            if (tempsum >= area0) {
                tempsum -= area0;
            } else if (tempsum < 0) {
                tempsum += area0;
            }
        }
        return new PolygonResult(tnum, perimeter, 0 + tempsum);
    }

    /**
     * Return the results assuming a tentative final test point is added via an azimuth and distance;
     * however, the data for the test point is not saved.
     * This lets you report a running result for the perimeter and area as the user moves the mouse
     * cursor. Ordinary floating point arithmetic is used to accumulate the data for the test point;
     * thus the area and perimeter returned are less accurate than if addPoint and compute are used.
     *
     * @param azi     azimuth at current point (degrees).
     * @param s       distance from current point to final test point (meters).
     * @param reverse if true then clockwise (instead of counter-clockwise) traversal counts as a
     *                positive area.
     * @param sign    if true then return a signed result for the area if the polygon is traversed in
     *                the "wrong" direction instead of returning the area for the rest of the earth.
     * @return PolygonResult(<i>num</i>, <i>perimeter</i>, <i>area</i>) where <i>num</i> is the
     * number of vertices, <i>perimeter</i> is the perimeter of the polygon or the length of the
     * polyline (meters), and <i>area</i> is the area of the polygon (meters<sup>2</sup>) or
     * Double.NaN of <i>polyline</i> is true in the constructor.
     */
    public PolygonResult testEdge(

protected HomogeneousLinearLeastSquaresLaterationSolver(
            final P[] positions, final double[] distances, final LaterationSolverListener<P> listener) {
        super(positions, distances, listener);
    }

    /**
     * Solves the lateration problem.
     *
     * @throws LaterationException if lateration fails.
     * @throws NotReadyException   if solver is not ready.
     * @throws LockedException     if instance is busy solving the lateration problem.
     */
    @Override
    public void solve() throws LaterationException, NotReadyException, LockedException {
        // The implementation on this method follows the algorithm  bellow.

        // Having 3 2D circles:
        // c1x, c1y, r1
        // c2x, c2y, r2
        // c3x, c3y, r3
        // where (c1x, c1y) are the coordinates of 1st circle center and r1 is its radius.
        // (c2x, c2y) are the coordinates of 2nd circle center and r2 is its radius.
        // (c3x, c3y) are the coordinates of 3rd circle center and r3 is its radius.

        // The equations of the circles are as follows:
        // (x - c1x)^2 + (y - c1y)^2 = r1^2
        // (x - c2x)^2 + (y - c2y)^2 = r2^2
        // (x - c3x)^2 + (y - c3y)^2 = r3^2

        // x^2 - 2*c1x*x + c1x^2 + y^2 - 2*c1y*y + c1y^2 = r1^2
        // x^2 - 2*c2x*x + c2x^2 + y^2 - 2*c2y*y + c2y^2 = r2^2
        // x^2 - 2*c3x*x + c3x^2 + y^2 - 2*c3y*y + c3y^2 = r3^2

        // remove 1st equation from others (we use 1st point as reference)

        // x^2 - 2*c2x*x + c2x^2 + y^2 - 2*c2y*y + c2y^2 - (x^2 - 2*c1x*x + c1x^2 + y^2 - 2*c1y*y + c1y^2) = r2^2 - r1^2
        // x^2 - 2*c3x*x + c3x^2 + y^2 - 2*c3y*y + c3y^2 - (x^2 - 2*c1x*x + c1x^2 + y^2 - 2*c1y*y + c1y^2) = r3^2 - r1^2

        // - 2*c2x*x + c2x^2 - 2*c2y*y + c2y^2 + 2*c1x*x - c1x^2 + 2*c1y*y - c1y^2 = r2^2 - r1^2
        // - 2*c3x*x + c3x^2 - 2*c3y*y + c3y^2 + 2*c1x*x - c1x^2 + 2*c1y*y - c1y^2 = r3^2 - r1^2

        // 2*(c1x - c2x)*x + c2x^2 + 2*(c1y - c2y)*y + c2y^2 - c1x^2 - c1y^2 = r2^2 - r1^2
        // 2*(c1x - c3x)*x + c3x^2 + 2*(c1y - c3y)*y + c3y^2 - c1x^2 - c1y^2 = r3^2 - r1^2

        // 2*(c1x - c2x)*x + 2*(c1y - c2y)*y = r2^2 - r1^2 + c1x^2 + c1y^2 - c2x^2 - c2y^2
        // 2*(c1x - c3x)*x + 2*(c1y - c3y)*y = r3^2 - r1^2 + c1x^2 + c1y^2 - c3x^2 - c3y^2

        // x and y are the inhomogeneous coordinates of the point (x,y) we want to find, we
        // substitute such point by the corresponding homogeneous coordinates (x,y) = (x'/w', y'/w')

        // Hence
        // 2*(c1x - c2x)*x'/w' + 2*(c1y - c2y)*y'/w' = r2^2 - r1^2 + c1x^2 + c1y^2 - c2x^2 - c2y^2
        // 2*(c1x - c3x)*x'/w' + 2*(c1y - c3y)*y'/w' = r3^2 - r1^2 + c1x^2 + c1y^2 - c3x^2 - c3y^2

        // Multiplying by w' at both sides...
        // 2*(c1x - c2x)*x' + 2*(c1y - c2y)*y' = (r2^2 - r1^2 + c1x^2 + c1y^2 - c2x^2 - c2y^2)*w'
        // 2*(c1x - c3x)*x' + 2*(c1y - c3y)*y' = (r3^2 - r1^2 + c1x^2 + c1y^2 - c3x^2 - c3y^2)*w'

        // Obtaining the following homogeneous equations
        // 2*(c1x - c2x)*x' + 2*(c1y - c2y)*y' - (r2^2 - r1^2 + c1x^2 + c1y^2 - c2x^2 - c2y^2)*w' = 0
        // 2*(c1x - c3x)*x' + 2*(c1y - c3y)*y' - (r3^2 - r1^2 + c1x^2 + c1y^2 - c3x^2 - c3y^2)*w' = 0

        // Fixing signs...
        // 2*(c1x - c2x)*x' + 2*(c1y - c2y)*y' + (r1^2 - r2^2 + c2x^2 + c2y^2 - c1x^2 - c1y^2)*w' = 0
        // 2*(c1x - c3x)*x' + 2*(c1y - c3y)*y' + (r1^2 - r3^2 + c3x^2 + c3y^2 - c1x^2 - c1y^2)*w' = 0


        // The homogeneous equations can be expressed as a linear system of homogeneous equations A*x = 0
        // where the unknowns to be solved are (x', y', w') up to scale.

        // [2*(c1x - c2x)   2*(c1y - c2y)    r1^2 - r2^2 + c2x^2 + c2y^2 - c1x^2 - c1y^2][x'] = 0
        // [2*(c1x - c3x)   2*(c1y - c3y)    r1^2 - r3^2 + c3x^2 + c3y^2 - c1x^2 - c1y^2][y'] = 0
        //                                                                               [w']

        // This can be solved by using the SVD decomposition of matrix A and picking the last column of
        // resulting V matrix. At least 2 equations are required to find a solution, since 1 additional
        // point is used as a reference, at least 3 points are required.

        // For spheres the solution is analogous

        // Having 4 3D spheres:
        // c1x, c1y, c1z, r1
        // c2x, c2y, c2z, r2
        // c3x, c3y, c3z, r3
        // c4x, c4y, c4z, r4
        // where (c1x, c1y, c1z) are the coordinates of 1st sphere center and r1 is its radius.
        // (c2x, c2y, c2z) are the coordinates of 2nd sphere center and r2 is its radius.
        // (c3x, c3y, c3z) are the coordinates of 3rd sphere center and r3 is its radius.
        // (c4x, c4y, c4z) are the coordinates of 4th sphere center and r4 is its radius.

        // The equations of the spheres are as follows:
        // (x - c1x)^2 + (y - c1y)^2 + (z - c1z)^2 = r1^2
        // (x - c2x)^2 + (y - c2y)^2 + (z - c2z)^2 = r2^2
        // (x - c3x)^2 + (y - c3y)^2 + (z - c3z)^2 = r3^2
        // (x - c4x)^2 + (y - c4y)^2 + (z - c4z)^2 = r4^2

        // x^2 - 2*c1x*x + c1x^2 + y^2 - 2*c1y*y + c1y^2 + z^2 - 2*c1z*z + c1z^2 = r1^2
        // x^2 - 2*c2x*x + c2x^2 + y^2 - 2*c2y*y + c2y^2 + z^2 - 2*c2z*z + c2z^2 = r2^2
        // x^2 - 2*c3x*x + c3x^2 + y^2 - 2*c3y*y + c3y^3 + z^2 - 2*c3z*z + c3z^2 = r3^2
        // x^2 - 2*c4x*x + c4x^2 + y^2 - 2*c4y*y + c4y^2 + z^2 - 2*c4z*z + c4z^2 = r4^2

        // remove 1st equation from others (we use 1st point as reference)
        // x^2 - 2*c2x*x + c2x^2 + y^2 - 2*c2y*y + c2y^2 + z^2 - 2*c2z*z + c2z^2 - (x^2 - 2*c1x*x + c1x^2 + y^2 - 2*c1y*y + c1y^2 + z^2 - 2*c1z*z + c1z^2) = r2^2 - r1^2
        // x^2 - 2*c3x*x + c3x^2 + y^2 - 2*c3y*y + c3y^3 + z^2 - 2*c3z*z + c3z^2 - (x^2 - 2*c1x*x + c1x^2 + y^2 - 2*c1y*y + c1y^2 + z^2 - 2*c1z*z + c1z^2) = r3^2 - r1^2
        // x^2 - 2*c4x*x + c4x^2 + y^2 - 2*c4y*y + c4y^2 + z^2 - 2*c4z*z + c4z^2 - (x^2 - 2*c1x*x + c1x^2 + y^2 - 2*c1y*y + c1y^2 + z^2 - 2*c1z*z + c1z^2) = r4^2 - r1^2

        // - 2*c2x*x + c2x^2 - 2*c2y*y + c2y^2 - 2*c2z*z + c2z^2 + 2*c1x*x - c1x^2 + 2*c1y*y - c1y^2 + 2*c1z*z - c1z^2 = r2^2 - r1^2
        // - 2*c3x*x + c3x^2 - 2*c3y*y + c3y^3 - 2*c3z*z + c3z^2 + 2*c1x*x - c1x^2 + 2*c1y*y - c1y^2 + 2*c1z*z - c1z^2 = r3^2 - r1^2
        // - 2*c4x*x + c4x^2 - 2*c4y*y + c4y^2 - 2*c4z*z + c4z^2 + 2*c1x*x - c1x^2 + 2*c1y*y - c1y^2 + 2*c1z*z - c1z^2 = r4^2 - r1^2

        // 2*(c1x - c2x)*x + c2x^2 + 2*(c1y - c2y)*y + c2y^2 + 2*(c1z - c2z)*z + c2z^2 - c1x^2 - c1y^2 - c1z^2 = r2^2 - r1^2
        // 2*(c1x - c3x)*x + c3x^2 + 2*(c1y - c3y)*y + c3y^3 + 2*(c1z - c3z)*z + c3z^2 - c1x^2 - c1y^2 - c1z^2 = r3^2 - r1^2
        // 2*(c1x - c4x)*x + c4x^2 + 2*(c1y - c4y)*y + c4y^2 + 2*(c1z - c4z)*z + c4z^2 - c1x^2 - c1y^2 - c1z^2 = r4^2 - r1^2

        // 2*(c1x - c2x)*x + 2*(c1y - c2y)*y + 2*(c1z - c2z)*z = r2^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c2x^2 - c2y^2 - c2z^2
        // 2*(c1x - c3x)*x + 2*(c1y - c3y)*y + 2*(c1z - c3z)*z = r3^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c3x^2 - c3y^3 - c3z^2
        // 2*(c1x - c4x)*x + 2*(c1y - c4y)*y + 2*(c1z - c4z)*z = r4^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c4x^2 - c4y^2 - c4z^2

        // x, y and z the inhomogeneous coordinates of the point (x,y,z) we want to find,
        // we substitute such point by the corresponding homogeneous coordinates
        // (x, y, z) = (x'/w', y'/w', z'/w')

        // Hence
        // 2*(c1x - c2x)*x'/w' + 2*(c1y - c2y)*y'/w' + 2*(c1z - c2z)*z'/w' = r2^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c2x^2 - c2y^2 - c2z^2
        // 2*(c1x - c3x)*x'/w' + 2*(c1y - c3y)*y'/w' + 2*(c1z - c3z)*z'/w' = r3^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c3x^2 - c3y^3 - c3z^2
        // 2*(c1x - c4x)*x'/w' + 2*(c1y - c4y)*y'/w' + 2*(c1z - c4z)*z'/w' = r4^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c4x^2 - c4y^2 - c4z^2

        // Multiplying by w' at both sides...
        // 2*(c1x - c2x)*x' + 2*(c1y - c2y)*y' + 2*(c1z - c2z)*z' = (r2^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c2x^2 - c2y^2 - c2z^2)*w'
        // 2*(c1x - c3x)*x' + 2*(c1y - c3y)*y' + 2*(c1z - c3z)*z' = (r3^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c3x^2 - c3y^3 - c3z^2)*w'
        // 2*(c1x - c4x)*x' + 2*(c1y - c4y)*y' + 2*(c1z - c4z)*z' = (r4^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c4x^2 - c4y^2 - c4z^2)*w'

        // Obtaining the following homogeneous equations
        // 2*(c1x - c2x)*x' + 2*(c1y - c2y)*y' + 2*(c1z - c2z)*z' - (r2^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c2x^2 - c2y^2 - c2z^2)*w' = 0
        // 2*(c1x - c3x)*x' + 2*(c1y - c3y)*y' + 2*(c1z - c3z)*z' - (r3^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c3x^2 - c3y^3 - c3z^2)*w' = 0
        // 2*(c1x - c4x)*x' + 2*(c1y - c4y)*y' + 2*(c1z - c4z)*z' - (r4^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c4x^2 - c4y^2 - c4z^2)*w' = 0

        // Fixing signs...
        // 2*(c1x - c2x)*x' + 2*(c1y - c2y)*y' + 2*(c1z - c2z)*z' + (r1^2 - r2^2 + c2x^2 + c2y^2 + c2z^2 - c1x^2 - c1y^2 - c1z^2)*w' = 0
        // 2*(c1x - c3x)*x' + 2*(c1y - c3y)*y' + 2*(c1z - c3z)*z' + (r1^2 - r3^2 + c3x^2 + c3y^3 + c3z^2 - c1x^2 - c1y^2 - c1z^2)*w' = 0
        // 2*(c1x - c4x)*x' + 2*(c1y - c4y)*y' + 2*(c1z - c4z)*z' + (r1^2 - r4^2 + c4x^2 + c4y^2 + c4z^2 - c1x^2 - c1y^2 - c1z^2)*w' = 0

        // The homogeneous equations can be expressed as a linear system of homogeneous equations
        // where the unknowns to be solved are (x', y', z', w') up to scale.

        // [2*(c1x - c2x)   2*(c1y - c2y)   2*(c1z - c2z)   r1^2 - r2^2 + c2x^2 + c2y^2 + c2z^2 - c1x^2 - c1y^2 - c1z^2][x'] = 0
        // [2*(c1x - c3x)   2*(c1y - c3y)   2*(c1z - c3z)   r1^2 - r3^2 + c3x^2 + c3y^3 + c3z^2 - c1x^2 - c1y^2 - c1z^2][y'] = 0
        // [2*(c1x - c4x)   2*(c1y - c4y)   2*(c1z - c4z)   r1^2 - r4^2 + c4x^2 + c4y^2 + c4z^2 - c1x^2 - c1y^2 - c1z^2][z'] = 0
        //                                                                                                              [w']

        // This can be solved by using the SVD decomposition of matrix A and picking the last column of
        // resulting V matrix. At least 3 equations are required to find a solution, since 1 additional
        // point is used as a reference, at least 4 points are required.

        if (!isReady()) {
            throw new NotReadyException();
        }
        if (isLocked()) {
            throw new LockedException();
        }

        try {
            locked = true;

            if (listener != null) {
                listener.onSolveStart(this);
            }

            final var numberOfPositions = positions.length;
            final var numberOfPositionsMinus1 = numberOfPositions - 1;
            final var dims = getNumberOfDimensions();

            final var referenceDistance = distances[0];

final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<Point2D>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return distances.length;
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Point2D> solutions) {
                solvePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final Point2D currentEstimation, final int i) {
                return Math.abs(currentEstimation.distanceTo(positions[i]) - distances[i]);
            }

            @Override
            public boolean isReady() {
                return MSACRobustLateration2DSolver.this.isReady();

final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<Point3D>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return distances.length;
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Point3D> solutions) {
                solvePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final Point3D currentEstimation, final int i) {
                return Math.abs(currentEstimation.distanceTo(positions[i]) - distances[i]);
            }

            @Override
            public boolean isReady() {
                return MSACRobustLateration3DSolver.this.isReady();

vz = velocity.getVz();
    }

    /**
     * Gets cartesian position and velocity.
     *
     * @param result instance where cartesian position and velocity will be stored.
     */
    public void getPositionAndVelocity(final ECEFPositionAndVelocity result) {
        result.setPositionCoordinates(x, y, z);
        result.setVelocityCoordinates(vx, vy, vz);
    }

    /**
     * Gets cartesian position and velocity.
     *
     * @return cartesian position and velocity.
     */
    public ECEFPositionAndVelocity getPositionAndVelocity() {
        return new ECEFPositionAndVelocity(x, y, z, vx, vy, vz);
    }

    /**
     * Sets cartesian position and velocity.
     *
     * @param positionAndVelocity cartesian position and velocity.
     */
    public void setPositionAndVelocity(final ECEFPositionAndVelocity positionAndVelocity) {
        x = positionAndVelocity.getX();
        y = positionAndVelocity.getY();
        z = positionAndVelocity.getZ();

        vx = positionAndVelocity.getVx();
        vy = positionAndVelocity.getVy();
        vz = positionAndVelocity.getVz();
    }

return new Distance(getPositionNorm(), DistanceUnit.METER);
    }

    /**
     * Gets x coordinate of velocity of body frame expressed in meters per second (m/s) resolved along ECI or
     * ECEF-frame axes.
     *
     * @return x coordinate of velocity.
     */
    public double getVx() {
        return vx;
    }

    /**
     * Sets x coordinate of velocity of body frame expressed in meters per second (m/s) resolved along ECI or
     * ECEF-frame axes.
     *
     * @param vx x coordinate of velocity.
     */
    public void setVx(final double vx) {
        this.vx = vx;
    }

    /**
     * Gets y coordinate of velocity of body frame expressed in meters per second (m/s) resolved along ECI or
     * ECEF-frame axes.
     *
     * @return y coordinate of velocity.
     */
    public double getVy() {
        return vy;
    }

    /**
     * Sets y coordinate of velocity of body frame expressed in meters per second (m/s) resolved along ECI or
     * ECEF-frame axes.
     *
     * @param vy y coordinate of velocity.
     */
    public void setVy(final double vy) {
        this.vy = vy;
    }

    /**
     * Gets z coordinate of velocity of body frame expressed in meters per second (m/s) resolved along ECI or
     * ECEF-frame axes.
     *
     * @return z coordinate of velocity.
     */
    public double getVz() {
        return vz;
    }

    /**
     * Sets z coordinate of velocity of body frame expressed in meters per second (m/s) resolved along ECI or
     * ECEF-frame axes.
     *
     * @param vz z coordinate of velocity.
     */
    public void setVz(final double vz) {
        this.vz = vz;
    }

    /**
     * Sets velocity coordinates of body frame expressed in meters per second (m/s) resolved along ECI or
     * ECEF-frame axes.
     *
     * @param vx x coordinate of velocity.
     * @param vy y coordinate of velocity.
     * @param vz z coordinate of velocity.
     */
    public void setVelocityCoordinates(final double vx, final double vy, final double vz) {
        this.vx = vx;
        this.vy = vy;
        this.vz = vz;
    }

    /**
     * Gets norm of velocity expressed in meters per second (m/s), which represents
     * the speed of the body.
     *
     * @return norm of velocity expressed in meters per second (m/s).
     */
    public double getVelocityNorm() {

}

    /**
     * Gets x coordinate of velocity of body frame expressed in meters per second (m/s) resolved along ECI or
     * ECEF-frame axes.
     *
     * @return x coordinate of velocity.
     */
    public double getVx() {
        return vx;
    }

    /**
     * Sets x coordinate of velocity of body frame expressed in meters per second (m/s) resolved along ECI or
     * ECEF-frame axes.
     *
     * @param vx x coordinate of velocity.
     */
    public void setVx(final double vx) {
        this.vx = vx;
    }

    /**
     * Gets y coordinate of velocity of body frame expressed in meters per second (m/s) resolved along ECI or
     * ECEF-frame axes.
     *
     * @return y coordinate of velocity.
     */
    public double getVy() {
        return vy;
    }

    /**
     * Sets y coordinate of velocity of body frame expressed in meters per second (m/s) resolved along ECI or
     * ECEF-frame axes.
     *
     * @param vy y coordinate of velocity.
     */
    public void setVy(final double vy) {
        this.vy = vy;
    }

    /**
     * Gets z coordinate of velocity of body frame expressed in meters per second (m/s) resolved along ECI or
     * ECEF-frame axes.
     *
     * @return z coordinate of velocity.
     */
    public double getVz() {
        return vz;
    }

    /**
     * Sets z coordinate of velocity of body frame expressed in meters per second (m/s) resolved along ECI or
     * ECEF-frame axes.
     *
     * @param vz z coordinate of velocity.
     */
    public void setVz(final double vz) {
        this.vz = vz;
    }

    /**
     * Sets velocity coordinates of body frame expressed in meters per second (m/s) resolved along ECI or
     * ECEF-frame axes.
     *
     * @param vx x coordinate of velocity.
     * @param vy y coordinate of velocity.
     * @param vz z coordinate of velocity.
     */
    public void setVelocityCoordinates(final double vx, final double vy, final double vz) {
        this.vx = vx;
        this.vy = vy;
        this.vz = vz;
    }

    /**
     * Gets norm of velocity expressed in meters per second (m/s), which represents
     * the speed of the body.
     *
     * @return norm of velocity expressed in meters per second (m/s).
     */
    public double getVelocityNorm() {

}

    /**
     * Gets x coordinate of velocity resolved in ECEF axes and expressed in
     * meters per second (m/s).
     *
     * @return x coordinate of velocity resolved in ECEF axes.
     */
    public double getVx() {
        return vx;
    }

    /**
     * Sets x coordinate of velocity resolved in ECEF axes and expressed in
     * meters per second (m/s).
     *
     * @param vx x coordinate of velocity resolved in ECEF axes.
     */
    public void setVx(final double vx) {
        this.vx = vx;
    }

    /**
     * Gets y coordinate of velocity resolved in ECEF axes and expressed in
     * meters per second (m/s).
     *
     * @return y coordinate of velocity resolved in ECEF axes.
     */
    public double getVy() {
        return vy;
    }

    /**
     * Sets y coordinate of velocity resolved in ECEF axes and expressed in
     * meters per second (m/s).
     *
     * @param vy y coordinate of velocity resolved in ECEf axes.
     */
    public void setVy(final double vy) {
        this.vy = vy;
    }

    /**
     * Gets z coordinate of velocity resolved in ECEF axes and expressed in
     * meters per second (m/s).
     *
     * @return z coordinate of velocity resolved in ECEF axes.
     */
    public double getVz() {
        return vz;
    }

    /**
     * Sets z coordinate of velocity resolved in ECEF axes and expressed in
     * meters per second (m/s).
     *
     * @param vz z coordinate of velocity resolved in ECEF axes.
     */
    public void setVz(final double vz) {
        this.vz = vz;
    }

    /**
     * Sets velocity coordinates resolved in ECEF axes and expressed in
     * meters per second (m/s).
     *
     * @param vx x coordinate of velocity.
     * @param vy y coordinate of velocity.
     * @param vz z coordinate of velocity.
     */
    public void setVelocityCoordinates(final double vx, final double vy, final double vz) {
        this.vx = vx;
        this.vy = vy;
        this.vz = vz;
    }

    /**
     * Gets x coordinate of velocity resolved in ECEF axes.
     *
     * @param result instance where x coordinate of velocity will be stored.
     */
    public void getSpeedX(final Speed result) {

}

    /**
     * Internally sets circles defining positions and Euclidean distances.
     *
     * @param circles circles defining positions and distances.
     * @throws IllegalArgumentException if circles is null or length of array of circles
     *                                  is less than 3.
     */
    private void internalSetCircles(final Circle[] circles) {
        if (circles == null || circles.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        final var positions = new Point2D[circles.length];
        final var distances = new double[circles.length];
        for (var i = 0; i < circles.length; i++) {
            final var circle = circles[i];
            positions[i] = circle.getCenter();
            distances[i] = circle.getRadius();
        }

        internalSetPositionsAndDistances(positions, distances);
    }

}

    /**
     * Internally sets spheres defining positions and Euclidean distances.
     *
     * @param spheres spheres defining positions and distances.
     * @throws IllegalArgumentException if spheres is null or length of array of spheres
     *                                  is less than 4.
     */
    private void internalSetSpheres(final Sphere[] spheres) {
        if (spheres == null || spheres.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        final var positions = new Point3D[spheres.length];
        final var distances = new double[spheres.length];
        for (var i = 0; i < spheres.length; i++) {
            final var sphere = spheres[i];
            positions[i] = sphere.getCenter();
            distances[i] = sphere.getRadius();
        }

        internalSetPositionsAndDistances(positions, distances);
    }

}

    /**
     * Internally sets circles defining positions and Euclidean distances.
     *
     * @param circles circles defining positions and distances.
     * @throws IllegalArgumentException if circles is null or length of array of circles
     *                                  is less than 3.
     */
    private void internalSetCircles(final Circle[] circles) {
        if (circles == null || circles.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        final var positions = new Point2D[circles.length];
        final var distances = new double[circles.length];
        for (var i = 0; i < circles.length; i++) {
            final var circle = circles[i];
            positions[i] = circle.getCenter();
            distances[i] = circle.getRadius();
        }

        internalSetPositionsAndDistances(positions, distances);
    }

}

    /**
     * Internally sets spheres defining positions and Euclidean distances.
     *
     * @param spheres spheres defining positions and distances.
     * @throws IllegalArgumentException if spheres is null or length of array of spheres
     *                                  is less than 4.
     */
    private void internalSetSpheres(final Sphere[] spheres) {
        if (spheres == null || spheres.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        final var positions = new Point3D[spheres.length];
        final var distances = new double[spheres.length];
        for (var i = 0; i < spheres.length; i++) {
            final var sphere = spheres[i];
            positions[i] = sphere.getCenter();
            distances[i] = sphere.getRadius();
        }

        internalSetPositionsAndDistances(positions, distances);
    }

private void internalSetSpheres(final Sphere[] spheres) {
        if (spheres == null || spheres.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        final var positions = new Point3D[spheres.length];
        final var distances = new double[spheres.length];
        for (var i = 0; i < spheres.length; i++) {
            final var sphere = spheres[i];
            positions[i] = sphere.getCenter();
            distances[i] = sphere.getRadius();
        }

        internalSetPositionsAndDistances(positions, distances);
    }

private void internalSetSpheres(final Sphere[] spheres) {
        if (spheres == null || spheres.length < getMinRequiredPositionsAndDistances()) {
            throw new IllegalArgumentException();
        }

        final var positions = new Point3D[spheres.length];
        final var distances = new double[spheres.length];
        for (var i = 0; i < spheres.length; i++) {
            final var sphere = spheres[i];
            positions[i] = sphere.getCenter();
            distances[i] = sphere.getRadius();
        }

        internalSetPositionsAndDistances(positions, distances);
    }