import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer; //导入依赖的package包/类
public double[] getLocation(){
    NonLinearLeastSquaresSolver solver = new NonLinearLeastSquaresSolver(new TrilaterationFunction(mPositions, mDistances), new LevenbergMarquardtOptimizer());
    LeastSquaresOptimizer.Optimum optimum = solver.solve();


    double[] centroid = optimum.getPoint().toArray();
    Log.d("centroid",centroid.length+"");
    Log.d("centroid",centroid[0]+" : "+centroid[1]+"  :  "+centroid[2]+"");
    return centroid;
}
 

import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer; //导入依赖的package包/类
@Test
public void findOptimum_realLocations_matchingOptimum() throws Exception {
    double[] expectedCenter = new double[]{6893722.565857311, 1473817.181344815}; // SOCCER_FIELD_CENTER
    double[][] positions = new double[][]{
            {6893765.004164174, 1473724.674847966}, // SOCCER_FIELD_TOP_LEFT
            {6893789.51604695, 1473894.4370714256}, // SOCCER_FIELD_TOP_RIGHT
            {6893679.213163125, 1473911.580273008}, // SOCCER_FIELD_BOTTOM_RIGHT
            {6893654.335773319, 1473740.259576677} // SOCCER_FIELD_BOTTOM_LEFT
    };
    double distance = 125.095963; // distance from soccer field center to any edge
    double[] distances = new double[]{distance, distance, distance, distance};

    LeastSquaresOptimizer.Optimum optimum = Multilateration.findOptimum(positions, distances);
    double[] actualCenter = optimum.getPoint().toArray();

    assertEquals(expectedCenter[0], actualCenter[0], 5);
    assertEquals(expectedCenter[1], actualCenter[1], 5);

    Location actualCenterLocation = Multilateration.getLocation(optimum);
    double error = actualCenterLocation.getDistanceTo(LocationTest.SOCCER_FIELD_CENTER);
    System.out.println("Found center location: " + actualCenterLocation.generateGoogleMapsUri());
    System.out.println("Error to expected location: " + String.format("%.2f", error) + "m");
    assertEquals(0, error, 5);
}
 

import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer; //导入依赖的package包/类
public static Location getLocation(LeastSquaresOptimizer.Optimum optimum) {
    double[] centroid = optimum.getPoint().toArray();
    double latitude = SphericalMercatorProjection.yToLatitude(centroid[0]);
    double longitude = SphericalMercatorProjection.xToLongitude(centroid[1]);
    // TODO: add altitude
    return new Location(latitude, longitude);
}
 

import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer; //导入依赖的package包/类
public static float getAccuracy(LeastSquaresOptimizer.Optimum optimum) {
    RealVector standardDeviation = optimum.getSigma(0);
    float maximumDeviation = 0;
    for (double deviation : standardDeviation.toArray()) {
        maximumDeviation = (float) Math.max(maximumDeviation, deviation);
    }
    return maximumDeviation;
}
 

import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer; //导入依赖的package包/类
@Test
public void findOptimum_perfectDistances_perfectOptimum() throws Exception {
    double[] expectedCenter = new double[]{0, 0};
    double[][] positions = new double[][]{{-10, 10}, {10, 10}, {10, -10}, {-10, -10}};
    double distance = 7.07106781;
    double[] distances = new double[]{distance, distance, distance, distance};

    LeastSquaresOptimizer.Optimum optimum = Multilateration.findOptimum(positions, distances);
    double[] actualCenter = optimum.getPoint().toArray();

    assertEquals(expectedCenter[0], actualCenter[0], 0);
    assertEquals(expectedCenter[1], actualCenter[1], 0);
}
 

import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer; //导入依赖的package包/类
@Test
public void findOptimum_realisticDistances_realisticOptimum() throws Exception {
    double[] expectedCenter = new double[]{0, 0};
    double[][] positions = new double[][]{{-10, 10}, {10, 10}, {10, -10}, {-10, -10}};
    double[] distances = new double[]{7.02, 7.05, 7.09, 7.1}; // real distance to all point would be ~7.07

    LeastSquaresOptimizer.Optimum optimum = Multilateration.findOptimum(positions, distances);
    double[] actualCenter = optimum.getPoint().toArray();

    assertEquals(expectedCenter[0], actualCenter[0], 0.2);
    assertEquals(expectedCenter[1], actualCenter[1], 0.2);
}
 

import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer; //导入依赖的package包/类
/**
 * Optimi.
 */
public void Optimize() {
    double[] init = new double[3];
    init[0] = initX;
    init[1] = initY;
    init[2] = initR;
    LeastSquaresOptimizer.Optimum optimum = optimizer.optimize(
            builder(circle).maxIterations(100).start(init).build());
    final double[] paramFound = optimum.getPoint().toArray();
    this.x = paramFound[0];
    this.y = paramFound[1];
    this.r = paramFound[2];
}
 

import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer; //导入依赖的package包/类
public LeastSquaresOptimizer.Optimum findOptimum() {
    double[][] positions = getPositions(beacons);
    double[] distances = getDistances(beacons);
    return findOptimum(positions, distances);
}
 

import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer; //导入依赖的package包/类
public LeastSquaresOptimizer.Optimum getOptimum() {
    if (optimum == null) {
        optimum = findOptimum();
    }
    return optimum;
}
 

import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer; //导入依赖的package包/类
public NonLinearLeastSquaresSolver(TrilaterationFunction function, LeastSquaresOptimizer leastSquaresOptimizer) {
	this.function = function;
	this.leastSquaresOptimizer = leastSquaresOptimizer;
}
 

import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer; //导入依赖的package包/类
private Position trilaterate(ArrayLocations locations) {
    //Trilateration nonlinear weighted least squares

    //https://github.com/lemmingapex/Trilateration - MIT Licence
    //http://commons.apache.org/proper/commons-math/download_math.cgi


    double[][] positions = new double[locations.size()][3];
    double[] distances   = new double[locations.size()];
    int i = 0;
    while (i < locations.size()) {

        //Map projection is treated as Mercator for calcs
        //Convert lat,lng to meters and then back again
        //Altitude is in cm

        positions[i] = new double[]{WebMercator.latitudeToY(locations.get(i).lat),
                WebMercator.longitudeToX(locations.get(i).lng),locations.get(i).altitude};

        distances[i] = locations.get(i).distance;
        i++;
    }
    TrilaterationFunction trilaterationFunction = new TrilaterationFunction(positions, distances);
    NonLinearLeastSquaresSolver solver = new NonLinearLeastSquaresSolver(trilaterationFunction, new LevenbergMarquardtOptimizer());

    LeastSquaresOptimizer.Optimum optimum = solver.solve();
    double[] centroid = optimum.getPoint().toArray();

    double errorRadius = 0;
    boolean errorCalc = false;

    // Error and geometry information
    try {
        //Create new array without the altitude. Including altitude causes a
        //SingularMatrixException as it cannot invert the matrix.
        double[][] err_positions = new double[locations.size()][2];
        i = 0;
        while (i < locations.size()) {

            err_positions[i] = new double[]{positions[i][0],
                    positions[i][1]};
            i++;
        }
        trilaterationFunction = new TrilaterationFunction(err_positions, distances);
        solver = new NonLinearLeastSquaresSolver(trilaterationFunction, new LevenbergMarquardtOptimizer());

        optimum = solver.solve();
        RealVector standardDeviation = optimum.getSigma(0);
        //RealMatrix covarianceMatrix = optimum.getCovariances(0);

        errorRadius = ((standardDeviation.getEntry(0) + standardDeviation.getEntry(1)) / 2) * 100;
        errorCalc = true;

    } catch (Exception ex) {
        errorRadius = 0;
        errorCalc = false;
    }

    return new Position(WebMercator.yToLatitude(optimum.getPoint().toArray()[0]),
            WebMercator.xToLongitude(centroid[1]),centroid[2],
            errorRadius, errorCalc);

}
 

import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer; //导入依赖的package包/类
private Result getResults(LeastSquaresOptimizer.Optimum optimizerResults) {
	Result r = new Result();
	double cost = optimizerResults.getCost();

	r.sse = cost * cost;
	r.degreesOfFreedom = targetValues.size() - parameters.size();
	r.r2 = MathUtils.getR2(r.sse, targetValues);
	r.aic = MathUtils.getAic(parameters.size(), targetValues.size(), r.sse);

	for (int i = 0; i < parameters.size(); i++) {
		r.parameterValues.put(parameters.get(i), optimizerResults.getPoint().getEntry(i));
	}

	if (r.degreesOfFreedom <= 0) {
		return r;
	}

	r.mse = r.sse / r.degreesOfFreedom;
	r.rmse = Math.sqrt(r.mse);

	double[][] covMatrix;

	try {
		covMatrix = optimizerResults.getCovariances(COV_THRESHOLD).getData();
	} catch (SingularMatrixException e) {
		return r;
	}

	r.parameterStandardErrors = new LinkedHashMap<>();
	r.parameterTValues = new LinkedHashMap<>();
	r.parameterPValues = new LinkedHashMap<>();
	r.covariances = new LinkedHashMap<>();

	for (int i = 0; i < parameters.size(); i++) {
		double error = Math.sqrt(r.mse * covMatrix[i][i]);

		r.parameterStandardErrors.put(parameters.get(i), error);

		double tValue = optimizerResults.getPoint().getEntry(i) / error;

		r.parameterTValues.put(parameters.get(i), tValue);
		r.parameterPValues.put(parameters.get(i), MathUtils.getPValue(tValue, r.degreesOfFreedom));
	}

	for (int i = 0; i < parameters.size(); i++) {
		for (int j = 0; j < parameters.size(); j++) {
			r.covariances.put(new Pair<>(parameters.get(i), parameters.get(j)), r.mse * covMatrix[i][j]);
		}
	}

	return r;
}
 

import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer; //导入依赖的package包/类
/**
 * Creates an optimizer set up to fit the appropriate curve.
 * <p>
 * The default implementation uses a {@link LevenbergMarquardtOptimizer
 * Levenberg-Marquardt} optimizer.
 * </p>
 * @return the optimizer to use for fitting the curve to the
 * given {@code points}.
 */
protected LeastSquaresOptimizer getOptimizer() {
    return new LevenbergMarquardtOptimizer();
}