Repository: lemmingapex/trilateration
Branch: master
Commit: fdd7b34d7ed9
Files: 15
Total size: 35.3 KB

Directory structure:
gitextract_awm7ruvq/

├── .gitignore
├── .travis.yml
├── LICENSE
├── README.md
├── build.gradle
├── gradle/
│   └── wrapper/
│       ├── gradle-wrapper.jar
│       └── gradle-wrapper.properties
├── gradlew
├── gradlew.bat
├── settings.gradle
└── src/
    ├── main/
    │   └── java/
    │       └── com/
    │           └── lemmingapex/
    │               └── trilateration/
    │                   ├── LinearLeastSquaresSolver.java
    │                   ├── NonLinearLeastSquaresSolver.java
    │                   └── TrilaterationFunction.java
    └── test/
        └── java/
            └── com/
                └── lemmingapex/
                    └── trilateration/
                        ├── TrilaterationTest.java
                        └── TrilaterationTestCases.java

================================================
FILE CONTENTS
================================================

================================================
FILE: .gitignore
================================================
*.class

# Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio
*.iml

## Directory-based project format:
.idea/

# Generated files
.gradle/
build/
bin/
gen/


================================================
FILE: .travis.yml
================================================
language: java


================================================
FILE: LICENSE
================================================
The MIT License (MIT)

Copyright (c) 2017 Scott Wiedemann

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.



================================================
FILE: README.md
================================================
# Trilateration

[![Build Status](https://travis-ci.org/lemmingapex/trilateration.svg?branch=master)](https://travis-ci.org/lemmingapex/trilateration)
[![License](https://img.shields.io/badge/license-MIT%20License-blue.svg)](LICENSE)

![alt text](Trilateration.png)

Solves a formulation of n-D space trilateration problem using a nonlinear least squares optimizer.

**Input:** positions, distances  
**Output:** centroid with geometry and error  

Uses [Levenberg-Marquardt algorithm](http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm) from [Apache Commons Math](http://commons.apache.org/proper/commons-math/).

```java
double[][] positions = new double[][] { { 5.0, -6.0 }, { 13.0, -15.0 }, { 21.0, -3.0 }, { 12.4, -21.2 } };
double[] distances = new double[] { 8.06, 13.97, 23.32, 15.31 };

NonLinearLeastSquaresSolver solver = new NonLinearLeastSquaresSolver(new TrilaterationFunction(positions, distances), new LevenbergMarquardtOptimizer());
Optimum optimum = solver.solve();

// the answer
double[] centroid = optimum.getPoint().toArray();

// error and geometry information; may throw SingularMatrixException depending the threshold argument provided
RealVector standardDeviation = optimum.getSigma(0);
RealMatrix covarianceMatrix = optimum.getCovariances(0);
```

The multilateration problem can be formulated as an optimization problem and solved using [Non-linear least squares methods](https://en.wikipedia.org/wiki/Non-linear_least_squares).  A well-formed solution will be an ellipse in R<sup>2</sup>, or an ellipsoid in R<sup>3</sup>.  If you are only interested in a maximum likelihood point estimate, the centroid is also provided.  R<sup>2</sup> space requires at least 3 non-degenerate points and distances to obtain a unique region; and similarly R<sup>3</sup> space requires at least 4 non-degenerate points and distances to obtain a unique region.

## Getting Trilateration
To add a dependency on Trilateration using Maven, use the following:

```xml
<dependency>
    <groupId>com.lemmingapex.trilateration</groupId>
    <artifactId>trilateration</artifactId>
    <version>1.0.2</version>
</dependency>
```

To add a dependency using Gradle:

```
dependencies {
  implementation 'com.lemmingapex.trilateration:trilateration:1.0.2'
}
```

## Run the tests

### *nix

./gradlew clean  
./gradlew test -i

### Windows
./gradlew.bat clean  
./gradlew.bat test -i


================================================
FILE: build.gradle
================================================
/*
 * This build file was auto generated by running the Gradle 'init' task
 * by 'scott' at '10/12/15 12:35 PM' with Gradle 2.7
 *
 * This generated file contains a sample Java project to get you started.
 * For more details take a look at the Java Quickstart chapter in the Gradle
 * user guide available at http://gradle.org/docs/2.7/userguide/tutorial_java_projects.html
 */

// Apply the java plugin to add support for Java
apply plugin: 'java'
apply plugin: 'maven'
apply plugin: 'signing'

group 'com.lemmingapex.trilateration'
//archivesBaseName 'Trilateration'
version '1.0.2'

def remotePublish = false

task javadocJar(type: Jar, dependsOn:javadoc) {
    classifier = 'javadoc'
    from javadoc
}

task sourcesJar(type: Jar, dependsOn:classes) {
    classifier = 'sources'
    from sourceSets.main.allSource
}

artifacts {
    archives javadocJar, sourcesJar
}

signing {
    required {remotePublish}
    sign configurations.archives
}

uploadArchives {
    repositories {
        mavenDeployer {
            if (!remotePublish) {
                repository url: 'file://' + new File(System.getProperty('user.home'), '.m2/repository')
            } else {
                beforeDeployment { MavenDeployment deployment -> signing.signPom(deployment) }
                repository(url: "https://oss.sonatype.org/service/local/staging/deploy/maven2/") {
                    authentication(userName: ossrhUsername, password: ossrhPassword)
                }
                snapshotRepository(url: "https://oss.sonatype.org/content/repositories/snapshots/") {
                    authentication(userName: ossrhUsername, password: ossrhPassword)
                }

                pom.project {
                    name 'trilateration'
                    packaging 'jar'
                    description 'Solves a formulation of n-D space trilateration problem using a nonlinear least squares optimizer'
                    url 'https://github.com/lemmingapex/trilateration'

                    scm {
                        connection 'scm:git:git@github.com:lemmingapex/trilateration.git'
                        developerConnection 'scm:git:git@github.com:lemmingapex/trilateration.git'
                        url 'git@github.com:lemmingapex/trilateration.git'
                    }

                    licenses {
                        license {
                            name 'The MIT License (MIT)'
                            url 'https://raw.githubusercontent.com/lemmingapex/trilateration/master/LICENSE'
                        }
                    }

                    developers {
                        developer {
                            id 'lemmingapex'
                            name 'Scott Wiedemann'
                            email 'lemmingapex@gmail.com'
                            roles {
                                role 'developer'
                            }
                            timezone 'UTC-07'
                        }
                    }
                }
            }
        }
    }
}

// In this section you declare where to find the dependencies of your project
repositories {
    // Use 'jcenter' for resolving your dependencies.
    // You can declare any Maven/Ivy/file repository here.
    jcenter()
}

task wrapper(type: Wrapper) {
   gradleVersion = '2.14'
}

// In this section you declare the dependencies for your production and test code
dependencies {
    compile 'org.apache.commons:commons-math3:3.6.1'

    // Declare the dependency for your favourite test framework you want to use in your tests.
    // TestNG is also supported by the Gradle Test task. Just change the
    // testCompile dependency to testCompile 'org.testng:testng:6.8.1' and add
    // 'test.useTestNG()' to your build script.
    testCompile 'junit:junit:4.12'
}


================================================
FILE: gradle/wrapper/gradle-wrapper.properties
================================================
#Wed Jul 13 08:15:29 MDT 2016
distributionBase=GRADLE_USER_HOME
distributionPath=wrapper/dists
zipStoreBase=GRADLE_USER_HOME
zipStorePath=wrapper/dists
distributionUrl=https\://services.gradle.org/distributions/gradle-2.14-bin.zip


================================================
FILE: gradlew
================================================
#!/usr/bin/env bash

##############################################################################
##
##  Gradle start up script for UN*X
##
##############################################################################

# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
DEFAULT_JVM_OPTS=""

APP_NAME="Gradle"
APP_BASE_NAME=`basename "$0"`

# Use the maximum available, or set MAX_FD != -1 to use that value.
MAX_FD="maximum"

warn ( ) {
    echo "$*"
}

die ( ) {
    echo
    echo "$*"
    echo
    exit 1
}

# OS specific support (must be 'true' or 'false').
cygwin=false
msys=false
darwin=false
case "`uname`" in
  CYGWIN* )
    cygwin=true
    ;;
  Darwin* )
    darwin=true
    ;;
  MINGW* )
    msys=true
    ;;
esac

# For Cygwin, ensure paths are in UNIX format before anything is touched.
if $cygwin ; then
    [ -n "$JAVA_HOME" ] && JAVA_HOME=`cygpath --unix "$JAVA_HOME"`
fi

# Attempt to set APP_HOME
# Resolve links: $0 may be a link
PRG="$0"
# Need this for relative symlinks.
while [ -h "$PRG" ] ; do
    ls=`ls -ld "$PRG"`
    link=`expr "$ls" : '.*-> \(.*\)$'`
    if expr "$link" : '/.*' > /dev/null; then
        PRG="$link"
    else
        PRG=`dirname "$PRG"`"/$link"
    fi
done
SAVED="`pwd`"
cd "`dirname \"$PRG\"`/" >&-
APP_HOME="`pwd -P`"
cd "$SAVED" >&-

CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar

# Determine the Java command to use to start the JVM.
if [ -n "$JAVA_HOME" ] ; then
    if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
        # IBM's JDK on AIX uses strange locations for the executables
        JAVACMD="$JAVA_HOME/jre/sh/java"
    else
        JAVACMD="$JAVA_HOME/bin/java"
    fi
    if [ ! -x "$JAVACMD" ] ; then
        die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME

Please set the JAVA_HOME variable in your environment to match the
location of your Java installation."
    fi
else
    JAVACMD="java"
    which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.

Please set the JAVA_HOME variable in your environment to match the
location of your Java installation."
fi

# Increase the maximum file descriptors if we can.
if [ "$cygwin" = "false" -a "$darwin" = "false" ] ; then
    MAX_FD_LIMIT=`ulimit -H -n`
    if [ $? -eq 0 ] ; then
        if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then
            MAX_FD="$MAX_FD_LIMIT"
        fi
        ulimit -n $MAX_FD
        if [ $? -ne 0 ] ; then
            warn "Could not set maximum file descriptor limit: $MAX_FD"
        fi
    else
        warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT"
    fi
fi

# For Darwin, add options to specify how the application appears in the dock
if $darwin; then
    GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\""
fi

# For Cygwin, switch paths to Windows format before running java
if $cygwin ; then
    APP_HOME=`cygpath --path --mixed "$APP_HOME"`
    CLASSPATH=`cygpath --path --mixed "$CLASSPATH"`

    # We build the pattern for arguments to be converted via cygpath
    ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null`
    SEP=""
    for dir in $ROOTDIRSRAW ; do
        ROOTDIRS="$ROOTDIRS$SEP$dir"
        SEP="|"
    done
    OURCYGPATTERN="(^($ROOTDIRS))"
    # Add a user-defined pattern to the cygpath arguments
    if [ "$GRADLE_CYGPATTERN" != "" ] ; then
        OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)"
    fi
    # Now convert the arguments - kludge to limit ourselves to /bin/sh
    i=0
    for arg in "$@" ; do
        CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -`
        CHECK2=`echo "$arg"|egrep -c "^-"`                                 ### Determine if an option

        if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then                    ### Added a condition
            eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"`
        else
            eval `echo args$i`="\"$arg\""
        fi
        i=$((i+1))
    done
    case $i in
        (0) set -- ;;
        (1) set -- "$args0" ;;
        (2) set -- "$args0" "$args1" ;;
        (3) set -- "$args0" "$args1" "$args2" ;;
        (4) set -- "$args0" "$args1" "$args2" "$args3" ;;
        (5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;;
        (6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;;
        (7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;;
        (8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;;
        (9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;;
    esac
fi

# Split up the JVM_OPTS And GRADLE_OPTS values into an array, following the shell quoting and substitution rules
function splitJvmOpts() {
    JVM_OPTS=("$@")
}
eval splitJvmOpts $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS
JVM_OPTS[${#JVM_OPTS[*]}]="-Dorg.gradle.appname=$APP_BASE_NAME"

exec "$JAVACMD" "${JVM_OPTS[@]}" -classpath "$CLASSPATH" org.gradle.wrapper.GradleWrapperMain "$@"


================================================
FILE: gradlew.bat
================================================
@if "%DEBUG%" == "" @echo off
@rem ##########################################################################
@rem
@rem  Gradle startup script for Windows
@rem
@rem ##########################################################################

@rem Set local scope for the variables with windows NT shell
if "%OS%"=="Windows_NT" setlocal

@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
set DEFAULT_JVM_OPTS=

set DIRNAME=%~dp0
if "%DIRNAME%" == "" set DIRNAME=.
set APP_BASE_NAME=%~n0
set APP_HOME=%DIRNAME%

@rem Find java.exe
if defined JAVA_HOME goto findJavaFromJavaHome

set JAVA_EXE=java.exe
%JAVA_EXE% -version >NUL 2>&1
if "%ERRORLEVEL%" == "0" goto init

echo.
echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
echo.
echo Please set the JAVA_HOME variable in your environment to match the
echo location of your Java installation.

goto fail

:findJavaFromJavaHome
set JAVA_HOME=%JAVA_HOME:"=%
set JAVA_EXE=%JAVA_HOME%/bin/java.exe

if exist "%JAVA_EXE%" goto init

echo.
echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
echo.
echo Please set the JAVA_HOME variable in your environment to match the
echo location of your Java installation.

goto fail

:init
@rem Get command-line arguments, handling Windowz variants

if not "%OS%" == "Windows_NT" goto win9xME_args
if "%@eval[2+2]" == "4" goto 4NT_args

:win9xME_args
@rem Slurp the command line arguments.
set CMD_LINE_ARGS=
set _SKIP=2

:win9xME_args_slurp
if "x%~1" == "x" goto execute

set CMD_LINE_ARGS=%*
goto execute

:4NT_args
@rem Get arguments from the 4NT Shell from JP Software
set CMD_LINE_ARGS=%$

:execute
@rem Setup the command line

set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar

@rem Execute Gradle
"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS%

:end
@rem End local scope for the variables with windows NT shell
if "%ERRORLEVEL%"=="0" goto mainEnd

:fail
rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
rem the _cmd.exe /c_ return code!
if  not "" == "%GRADLE_EXIT_CONSOLE%" exit 1
exit /b 1

:mainEnd
if "%OS%"=="Windows_NT" endlocal

:omega


================================================
FILE: settings.gradle
================================================
/*
 * This settings file was auto generated by the Gradle buildInit task
 * by 'scott' at '10/12/15 12:35 PM' with Gradle 2.7
 *
 * The settings file is used to specify which projects to include in your build.
 * In a single project build this file can be empty or even removed.
 *
 * Detailed information about configuring a multi-project build in Gradle can be found
 * in the user guide at http://gradle.org/docs/2.7/userguide/multi_project_builds.html
 */

/*
// To declare projects as part of a multi-project build use the 'include' method
include 'shared'
include 'api'
include 'services:webservice'
*/

rootProject.name = 'trilateration'


================================================
FILE: src/main/java/com/lemmingapex/trilateration/LinearLeastSquaresSolver.java
================================================
package com.lemmingapex.trilateration;

import org.apache.commons.math3.linear.*;

/**
 *
 * For testing only. A linear approach to solve the Trilateration problem.
 * see http://inside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf
 *
 * @author scott
 */
public class LinearLeastSquaresSolver {

    protected final TrilaterationFunction function;

    public LinearLeastSquaresSolver(TrilaterationFunction function) {
        this.function = function;
    }

    public RealVector solve(boolean debugInfo) {
        int numberOfPositions = function.getPositions().length;
        int positionDimension = function.getPositions()[0].length;

        double[][] Ad = new double[numberOfPositions - 1][positionDimension];

        // TODO: which reference position should be used?  currently using postion and distance in index 0.

        for (int i = 1; i < numberOfPositions; i++) {
            double[] Adi = new double[positionDimension];
            for (int j = 0; j < positionDimension; j++) {
                Adi[j] = function.getPositions()[i][j] - function.getPositions()[0][j];
            }
            Ad[i - 1] = Adi;
        }
        if (debugInfo) {
            System.out.println(new Array2DRowRealMatrix(Ad));
        }

        // reference point is function.getPositions()[0], with distance function.getDistances()[0]
        double referenceDistance = function.getDistances()[0];
        double r0squared = referenceDistance * referenceDistance;
        double[] bd = new double[numberOfPositions - 1];
        for (int i = 1; i < numberOfPositions; i++) {
            double ri = function.getDistances()[i];
            double risquared = ri * ri;

            // find distance between ri and r0
            double di0squared = 0;
            for (int j = 0; j < positionDimension; j++) {
                double dij0j = function.getPositions()[i][j] - function.getPositions()[0][j];
                di0squared += dij0j * dij0j;
            }
            bd[i - 1] = 0.5 * (r0squared - risquared + di0squared);
        }
        if (debugInfo) {
            System.out.println(new ArrayRealVector(bd));
        }

        RealMatrix A = new Array2DRowRealMatrix(Ad, false);
        RealVector b = new ArrayRealVector(bd, false);
        DecompositionSolver solver = new QRDecomposition(A).getSolver();
        RealVector x;
        if(!solver.isNonSingular()) {
            // bummer...
            x = new ArrayRealVector(new double[positionDimension]);
        } else {
             x = solver.solve(b);
        }

        return x.add(new ArrayRealVector(function.getPositions()[0]));
    }

    public RealVector solve() {
        return solve(false);
    }
}


================================================
FILE: src/main/java/com/lemmingapex/trilateration/NonLinearLeastSquaresSolver.java
================================================
package com.lemmingapex.trilateration;

import org.apache.commons.math3.fitting.leastsquares.LeastSquaresFactory;
import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer;
import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer.Optimum;
import org.apache.commons.math3.fitting.leastsquares.LeastSquaresProblem;
import org.apache.commons.math3.linear.ArrayRealVector;
import org.apache.commons.math3.linear.DiagonalMatrix;

/**
 * Solves a Trilateration problem with an instance of a
 * {@link LeastSquaresOptimizer}
 *
 * @author scott
 *
 */
public class NonLinearLeastSquaresSolver {

	protected final TrilaterationFunction function;
	protected final LeastSquaresOptimizer leastSquaresOptimizer;

	protected final static int MAXNUMBEROFITERATIONS = 1000;

	public NonLinearLeastSquaresSolver(TrilaterationFunction function, LeastSquaresOptimizer leastSquaresOptimizer) {
		this.function = function;
		this.leastSquaresOptimizer = leastSquaresOptimizer;
	}

	public Optimum solve(double[] target, double[] weights, double[] initialPoint, boolean debugInfo) {
		if (debugInfo) {
			System.out.println("Max Number of Iterations : " + MAXNUMBEROFITERATIONS);
		}

		LeastSquaresProblem leastSquaresProblem = LeastSquaresFactory.create(
				// function to be optimized
				function,
				// target values at optimal point in least square equation
				// (x0+xi)^2 + (y0+yi)^2 + ri^2 = target[i]
				new ArrayRealVector(target, false), new ArrayRealVector(initialPoint, false), new DiagonalMatrix(weights), null, MAXNUMBEROFITERATIONS, MAXNUMBEROFITERATIONS);

		return leastSquaresOptimizer.optimize(leastSquaresProblem);
	}

	public Optimum solve(double[] target, double[] weights, double[] initialPoint) {
		return solve(target, weights, initialPoint, false);
	}

	public Optimum solve(boolean debugInfo) {
		int numberOfPositions = function.getPositions().length;
		int positionDimension = function.getPositions()[0].length;

		double[] initialPoint = new double[positionDimension];
		// initial point, use average of the vertices
		for (int i = 0; i < function.getPositions().length; i++) {
			double[] vertex = function.getPositions()[i];
			for (int j = 0; j < vertex.length; j++) {
				initialPoint[j] += vertex[j];
			}
		}
		for (int j = 0; j < initialPoint.length; j++) {
			initialPoint[j] /= numberOfPositions;
		}

		if (debugInfo) {
			StringBuilder output = new StringBuilder("initialPoint: ");
			for (int i = 0; i < initialPoint.length; i++) {
				output.append(initialPoint[i]).append(" ");
			}
			System.out.println(output.toString());
		}

		double[] target = new double[numberOfPositions];
		double[] distances = function.getDistances();
		double[] weights = new double[target.length];
		for (int i = 0; i < target.length; i++) {
			target[i] = 0.0;
			weights[i] = inverseSquareLaw(distances[i]);
		}

		return solve(target, weights, initialPoint, debugInfo);
	}

	private double inverseSquareLaw(double distance) {
		return 1 / (distance * distance);
	}

	public Optimum solve() {
		return solve(false);
	}
}


================================================
FILE: src/main/java/com/lemmingapex/trilateration/TrilaterationFunction.java
================================================
package com.lemmingapex.trilateration;

import org.apache.commons.math3.fitting.leastsquares.MultivariateJacobianFunction;
import org.apache.commons.math3.linear.Array2DRowRealMatrix;
import org.apache.commons.math3.linear.ArrayRealVector;
import org.apache.commons.math3.linear.RealMatrix;
import org.apache.commons.math3.linear.RealVector;
import org.apache.commons.math3.util.Pair;

/**
 * Models the Trilateration problem. This is a formulation for a nonlinear least
 * squares optimizer.
 *
 * @author scott
 *
 */
public class TrilaterationFunction implements MultivariateJacobianFunction {

	protected static final double epsilon = 1E-7;

	/**
	 * Known positions of static nodes
	 */
	protected final double positions[][];

	/**
	 * Euclidean distances from static nodes to mobile node
	 */
	protected final double distances[];

	public TrilaterationFunction(double positions[][], double distances[]) {

		if(positions.length < 2) {
			throw new IllegalArgumentException("Need at least two positions.");
		}

		if(positions.length != distances.length) {
			throw new IllegalArgumentException("The number of positions you provided, " + positions.length + ", does not match the number of distances, " + distances.length + ".");
		}

		// bound distances to strictly positive domain
		for (int i = 0; i < distances.length; i++) {
			distances[i] = Math.max(distances[i], epsilon);
		}

		int positionDimension = positions[0].length;
		for (int i = 1; i < positions.length; i++) {
			if(positionDimension != positions[i].length) {
				throw new IllegalArgumentException("The dimension of all positions should be the same.");
			}
		}

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

	public final double[] getDistances() {
		return distances;
	}

	public final double[][] getPositions() {
		return positions;
	}

	/**
	 * Calculate and return Jacobian function Actually return initialized function
	 *
	 * Jacobian matrix, [i][j] at
	 * J[i][0] = delta_[(x0-xi)^2 + (y0-yi)^2 - ri^2]/delta_[x0] at
	 * J[i][1] = delta_[(x0-xi)^2 + (y0-yi)^2 - ri^2]/delta_[y0] partial derivative with respect to the parameters passed to value() method
	 *
	 * @param point for which to calculate the slope
	 * @return Jacobian matrix for point
	 */
	public RealMatrix jacobian(RealVector point) {
		double[] pointArray = point.toArray();

		double[][] jacobian = new double[distances.length][pointArray.length];
		for (int i = 0; i < jacobian.length; i++) {
			for (int j = 0; j < pointArray.length; j++) {
				jacobian[i][j] = 2 * pointArray[j] - 2 * positions[i][j];
			}
		}

		return new Array2DRowRealMatrix(jacobian);
	}

	@Override
	public Pair<RealVector, RealMatrix> value(RealVector point) {

		// input
		double[] pointArray = point.toArray();

		// output
		double[] resultPoint = new double[this.distances.length];

		// compute least squares
		for (int i = 0; i < resultPoint.length; i++) {
			resultPoint[i] = 0.0;
			// calculate sum, add to overall
			for (int j = 0; j < pointArray.length; j++) {
				resultPoint[i] += (pointArray[j] - this.getPositions()[i][j]) * (pointArray[j] - this.getPositions()[i][j]);
			}
			resultPoint[i] -= (this.getDistances()[i]) * (this.getDistances()[i]);
		}

		RealMatrix jacobian = jacobian(point);
		return new Pair<RealVector, RealMatrix>(new ArrayRealVector(resultPoint), jacobian);
	}
}


================================================
FILE: src/test/java/com/lemmingapex/trilateration/TrilaterationTest.java
================================================
package com.lemmingapex.trilateration;

import static org.junit.Assert.assertEquals;

import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer.Optimum;
import org.apache.commons.math3.fitting.leastsquares.LevenbergMarquardtOptimizer;
import org.apache.commons.math3.linear.RealMatrix;
import org.apache.commons.math3.linear.RealVector;
import org.apache.commons.math3.linear.SingularMatrixException;

/**
 * Test class which is initialized with different predefined test cases.
 * Test was refactored from @author scott
 *
 * @author burfi
 */
public class TrilaterationTest {

	private double[][] positions;
	private double[] distances;
	private double[] expectedPosition;
	private double acceptedDelta;
	private StringBuilder output;

	private RealVector linearCalculatedPosition;
	private Optimum nonLinearOptimum;

	public TrilaterationTest(double[][] positions, double[] distances, double[] expectedPosition, double acceptedDelta) {
		this.positions = positions;
		this.distances = distances;
		this.expectedPosition = expectedPosition;
		this.acceptedDelta = acceptedDelta;
		testCase();
		outputResult();
		compareExpectedAndCalculatedResults();
	}

	private void testCase() {
		TrilaterationFunction trilaterationFunction = new TrilaterationFunction(positions, distances);
		LinearLeastSquaresSolver lSolver = new LinearLeastSquaresSolver(trilaterationFunction);
		NonLinearLeastSquaresSolver nlSolver = new NonLinearLeastSquaresSolver(trilaterationFunction, new LevenbergMarquardtOptimizer());

		linearCalculatedPosition = lSolver.solve();
		nonLinearOptimum = nlSolver.solve();
	}

	private void outputResult() {
		output = new StringBuilder();
		printDoubleArray("expectedPosition: ", expectedPosition);
		printDoubleArray("linear calculatedPosition: ", linearCalculatedPosition.toArray());
		printDoubleArray("non-linear calculatedPosition: ", nonLinearOptimum.getPoint().toArray());
		output.append("numberOfIterations: ").append(nonLinearOptimum.getIterations()).append("\n");
		output.append("numberOfEvaluations: ").append(nonLinearOptimum.getEvaluations()).append("\n");
		try {
			RealVector standardDeviation = nonLinearOptimum.getSigma(0);
			printDoubleArray("standardDeviation: ", standardDeviation.toArray());
			output.append("Norm of deviation: ").append(standardDeviation.getNorm()).append("\n");
			RealMatrix covarianceMatrix = nonLinearOptimum.getCovariances(0);
			output.append("covarianceMatrix: ").append(covarianceMatrix).append("\n");
		} catch (SingularMatrixException e) {
			System.err.println(e.getMessage());
		}

		System.out.println(output.toString());
	}

	private void compareExpectedAndCalculatedResults() {
		double[] calculatedPosition = nonLinearOptimum.getPoint().toArray();
		for (int i = 0; i < calculatedPosition.length; i++) {
			assertEquals(expectedPosition[i], calculatedPosition[i], acceptedDelta);
		}
	}

	private void printDoubleArray(String tag, double[] values) {
		output.append(tag);
		for (double p : values) {
			output.append(p).append(" ");
		}
		output.append("\n");
	}
}

================================================
FILE: src/test/java/com/lemmingapex/trilateration/TrilaterationTestCases.java
================================================
package com.lemmingapex.trilateration;

import org.junit.Test;

/**
 * Test class which is initialized with different predefined test cases.
 * All test cases were defined by @author scott
 *
 * @author burfi
 */
public class TrilaterationTestCases {

    @Test
    public void trilateration1DExact1() throws Exception {
        double[][] positions = new double[][]{{1.0}, {2.0}, {3.0}};
        double[] distances = new double[]{1.1, 0.1, 0.9};
        double[] expectedPosition = new double[]{2.1};
        double acceptedDelta = 0.0001;
        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);
    }

    @Test
    public void trilateration1DExact2() throws Exception {
        double[][] positions = new double[][]{{1000.0}, {2000.0}, {3000.0}};
        double[] distances = new double[]{1100, 100, 900};
        double[] expectedPosition = new double[]{2100.0};
        double acceptedDelta = 0.0001;
        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);
    }

    @Test
    public void trilateration1DInexact() throws Exception {
        double[][] positions = new double[][]{{1000.0}, {2000.0}, {3000.0}};
        double[] distances = new double[]{1110, 110, 910};
        double[] expectedPosition = new double[]{2100.0};
        double acceptedDelta = 30;
        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);
    }

    @Test
    public void trilateration2DExact1() throws Exception {
        double[][] positions = new double[][]{{1.0, 1.0}, {3.0, 1.0}, {2.0, 2.0}};
        double[] distances = new double[]{1.0, 1.0, 1.0};
        double[] expectedPosition = new double[]{2.0, 1.0};
        double acceptedDelta = 0.0001;
        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);
    }

    @Test
    public void trilateration2DZeroDistance() throws Exception {
        double[][] positions = new double[][]{{1.0, 1.0}, {2.0, 1.0}};
        double[] distances = new double[]{0.0, 1.0};
        double[] expectedPosition = new double[]{1.0, 1.0};
        double acceptedDelta = 0.0001;
        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);
    }

    @Test
    public void trilateration2DExact2() throws Exception {
        double[][] positions = new double[][]{{0.0, 0.0}, {-1.0, 0.0}, {0.0, -1.0}};
        double[] distances = new double[]{Math.sqrt(2.0), 1.0, 1.0};
        double[] expectedPosition = new double[]{-1.0, -1.0};
        double acceptedDelta = 0.0001;
        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);
    }

    @Test
    public void trilateration2DExact3() throws Exception {
        double[][] positions = new double[][]{{0.0, 0.0}, {1000.0, 0.0}, {0.0, 1000.0}};
        double[] distances = new double[]{Math.sqrt(2.0) * 1000.0, 1000.0, 1000.0};
        double[] expectedPosition = new double[]{1000.0, 1000.0};
        double acceptedDelta = 0.0001;
        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);
    }

    @Test
    public void trilateration2DExact4() throws Exception {
        double[][] positions = new double[][]{{1.0, 1.0}, {1.0, 3.0}, {8.0, 8.0}, {2.0, 2.0}};
        double[] distances = new double[]{5.0, 5.0, 6.36, 3.9};
        double[] expectedPosition = new double[]{5.9, 2.0};
        double acceptedDelta = 0.01;
        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);
    }

    @Test
    public void trilateration2DExact5() throws Exception {
        double[][] positions = new double[][]{{5.0, -6.0}, {13.0, -15.0}, {21.0, -3.0}};
        double[] distances = new double[]{8.06, 13.97, 23.32};
        double[] expectedPosition = new double[]{-0.6, -11.8};
        double acceptedDelta = 0.01;
        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);
    }

    @Test
    public void trilateration2DInexact1() throws Exception {
        double[][] positions = new double[][]{{1.0, 1.0}, {3.0, 1.0}, {2.0, 2.0}};
        double[] distances = new double[]{0.9, 1.0, 1.0};
        double[] expectedPosition = new double[]{2.0, 1.0};
        double acceptedDelta = 0.1;
        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);
    }

    @Test
    public void trilateration2DInexact2() throws Exception {
        double[][] positions = new double[][]{{5.0, -6.0}, {13.0, -15.0}, {21.0, -3.0}, {12.42, -21.2}};
        double[] distances = new double[]{8.06, 13.97, 23.32, 15.31};
        double[] expectedPosition = new double[]{-0.6, -11.8};
        double acceptedDelta = 1.0;
        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);
    }

    @Test
    public void trilateration2DNonIntersecting() throws Exception {
        double[][] positions = new double[][]{{1.0, 1.0}, {3.0, 1.0}, {2.0, 2.0}};
        double[] distances = new double[]{0.5, 0.5, 0.5};
        double[] expectedPosition = new double[]{2.0, 1.0};
        double acceptedDelta = 0.25;
        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);
    }

    @Test
    public void trilateration2DOverIntersecting() throws Exception {
        double[][] positions = new double[][]{{1.0, 1.0}, {3.0, 1.0}, {2.0, 2.0}};
        double[] distances = new double[]{2.0, 2.0, 2.0};
        double[] expectedPosition = new double[]{2.0, 1.0};
        double acceptedDelta = 2.0;
        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);
    }

    @Test
    public void trilateration2DDegenerateCase1() throws Exception {
        double[][] positions = new double[][]{{1.0, 1.0}, {1.0, 1.0}, {3.0, 1.0}};
        double[] distances = new double[]{1.0, 1.0, 1.0};
        double[] expectedPosition = new double[]{2.0, 1.0};
        double acceptedDelta = 0.5;
        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);
    }

    @Test
    public void trilateration2DDegenerateCase2() throws Exception {
        double[][] positions = new double[][]{{1.0, 1.0}, {1.0, 1.0}, {1.0, 1.0}};
        double[] distances = new double[]{1.0, 1.0, 1.0};
        double[] expectedPosition = new double[]{1.0, 1.0};
        double acceptedDelta = 0.5;
        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);
    }

    @Test
    public void trilateration2DUnderdertermined() throws Exception {
        double[][] positions = new double[][]{{1.0, 1.0}, {3.0, 1.0}};
        double[] distances = new double[]{1.0, 1.0};
        double[] expectedPosition = new double[]{2.0, 1.0};
        double acceptedDelta = 0.5;
        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);
    }

    @Test
    public void trilateration3DExact() throws Exception {
        double[][] positions = new double[][]{{1.0, 1.0, 1.0}, {3.0, 1.0, 1.0}, {2.0, 2.0, 1.0}};
        double[] distances = new double[]{1.0, 1.0, 1.0};
        double[] expectedPosition = new double[]{2.0, 1.0, 1.0};
        double acceptedDelta = 0.0001;
        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);
    }

    @Test
    public void trilateration3DInexact() throws Exception {
        double[][] positions = new double[][]{{0.0, 0.0, 0.0}, {8.84, 4.57, 12.59}, {0.0, -8.84, 8.84}, {10.72, -8.96, 8.84}};
        double[] distances = new double[]{8.84, 8.84, 8.84, 8.84};
        double[] expectedPosition = new double[]{5.2, -1.2, 7.7};
        double acceptedDelta = 1.0;
        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);
    }

    @Test
    public void trilateration4DInexact() throws Exception {
        double[][] positions = new double[][]{{0.0, 0.0, 0.0, 0.0}, {8.84, 4.57, 12.59, 9.2}, {0.0, -8.84, 8.84, 9.2}, {10.72, -8.96, 8.84, 9.2}};
        double[] distances = new double[]{8.84, 8.84, 8.84, 8.84};
        double[] expectedPosition = new double[]{5.2, -1.5, 7.7, 5.9};
        double acceptedDelta = 1.0;
        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);
    }
}