[
  {
    "path": ".gitignore",
    "content": "*.class\n\n# Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio\n*.iml\n\n## Directory-based project format:\n.idea/\n\n# Generated files\n.gradle/\nbuild/\nbin/\ngen/\n"
  },
  {
    "path": ".travis.yml",
    "content": "language: java\n"
  },
  {
    "path": "LICENSE",
    "content": "The MIT License (MIT)\n\nCopyright (c) 2017 Scott Wiedemann\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\nof this software and associated documentation files (the \"Software\"), to deal\nin the Software without restriction, including without limitation the rights\nto use, copy, modify, merge, publish, distribute, sublicense, and/or sell\ncopies of the Software, and to permit persons to whom the Software is\nfurnished to do so, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in all\ncopies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE\nSOFTWARE.\n\n"
  },
  {
    "path": "README.md",
    "content": "# Trilateration\n\n[![Build Status](https://travis-ci.org/lemmingapex/trilateration.svg?branch=master)](https://travis-ci.org/lemmingapex/trilateration)\n[![License](https://img.shields.io/badge/license-MIT%20License-blue.svg)](LICENSE)\n\n![alt text](Trilateration.png)\n\nSolves a formulation of n-D space trilateration problem using a nonlinear least squares optimizer.\n\n**Input:** positions, distances  \n**Output:** centroid with geometry and error  \n\nUses [Levenberg-Marquardt algorithm](http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm) from [Apache Commons Math](http://commons.apache.org/proper/commons-math/).\n\n```java\ndouble[][] positions = new double[][] { { 5.0, -6.0 }, { 13.0, -15.0 }, { 21.0, -3.0 }, { 12.4, -21.2 } };\ndouble[] distances = new double[] { 8.06, 13.97, 23.32, 15.31 };\n\nNonLinearLeastSquaresSolver solver = new NonLinearLeastSquaresSolver(new TrilaterationFunction(positions, distances), new LevenbergMarquardtOptimizer());\nOptimum optimum = solver.solve();\n\n// the answer\ndouble[] centroid = optimum.getPoint().toArray();\n\n// error and geometry information; may throw SingularMatrixException depending the threshold argument provided\nRealVector standardDeviation = optimum.getSigma(0);\nRealMatrix covarianceMatrix = optimum.getCovariances(0);\n```\n\nThe 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.\n\n## Getting Trilateration\nTo add a dependency on Trilateration using Maven, use the following:\n\n```xml\n<dependency>\n    <groupId>com.lemmingapex.trilateration</groupId>\n    <artifactId>trilateration</artifactId>\n    <version>1.0.2</version>\n</dependency>\n```\n\nTo add a dependency using Gradle:\n\n```\ndependencies {\n  implementation 'com.lemmingapex.trilateration:trilateration:1.0.2'\n}\n```\n\n## Run the tests\n\n### *nix\n\n./gradlew clean  \n./gradlew test -i\n\n### Windows\n./gradlew.bat clean  \n./gradlew.bat test -i\n"
  },
  {
    "path": "build.gradle",
    "content": "/*\n * This build file was auto generated by running the Gradle 'init' task\n * by 'scott' at '10/12/15 12:35 PM' with Gradle 2.7\n *\n * This generated file contains a sample Java project to get you started.\n * For more details take a look at the Java Quickstart chapter in the Gradle\n * user guide available at http://gradle.org/docs/2.7/userguide/tutorial_java_projects.html\n */\n\n// Apply the java plugin to add support for Java\napply plugin: 'java'\napply plugin: 'maven'\napply plugin: 'signing'\n\ngroup 'com.lemmingapex.trilateration'\n//archivesBaseName 'Trilateration'\nversion '1.0.2'\n\ndef remotePublish = false\n\ntask javadocJar(type: Jar, dependsOn:javadoc) {\n    classifier = 'javadoc'\n    from javadoc\n}\n\ntask sourcesJar(type: Jar, dependsOn:classes) {\n    classifier = 'sources'\n    from sourceSets.main.allSource\n}\n\nartifacts {\n    archives javadocJar, sourcesJar\n}\n\nsigning {\n    required {remotePublish}\n    sign configurations.archives\n}\n\nuploadArchives {\n    repositories {\n        mavenDeployer {\n            if (!remotePublish) {\n                repository url: 'file://' + new File(System.getProperty('user.home'), '.m2/repository')\n            } else {\n                beforeDeployment { MavenDeployment deployment -> signing.signPom(deployment) }\n                repository(url: \"https://oss.sonatype.org/service/local/staging/deploy/maven2/\") {\n                    authentication(userName: ossrhUsername, password: ossrhPassword)\n                }\n                snapshotRepository(url: \"https://oss.sonatype.org/content/repositories/snapshots/\") {\n                    authentication(userName: ossrhUsername, password: ossrhPassword)\n                }\n\n                pom.project {\n                    name 'trilateration'\n                    packaging 'jar'\n                    description 'Solves a formulation of n-D space trilateration problem using a nonlinear least squares optimizer'\n                    url 'https://github.com/lemmingapex/trilateration'\n\n                    scm {\n                        connection 'scm:git:git@github.com:lemmingapex/trilateration.git'\n                        developerConnection 'scm:git:git@github.com:lemmingapex/trilateration.git'\n                        url 'git@github.com:lemmingapex/trilateration.git'\n                    }\n\n                    licenses {\n                        license {\n                            name 'The MIT License (MIT)'\n                            url 'https://raw.githubusercontent.com/lemmingapex/trilateration/master/LICENSE'\n                        }\n                    }\n\n                    developers {\n                        developer {\n                            id 'lemmingapex'\n                            name 'Scott Wiedemann'\n                            email 'lemmingapex@gmail.com'\n                            roles {\n                                role 'developer'\n                            }\n                            timezone 'UTC-07'\n                        }\n                    }\n                }\n            }\n        }\n    }\n}\n\n// In this section you declare where to find the dependencies of your project\nrepositories {\n    // Use 'jcenter' for resolving your dependencies.\n    // You can declare any Maven/Ivy/file repository here.\n    jcenter()\n}\n\ntask wrapper(type: Wrapper) {\n   gradleVersion = '2.14'\n}\n\n// In this section you declare the dependencies for your production and test code\ndependencies {\n    compile 'org.apache.commons:commons-math3:3.6.1'\n\n    // Declare the dependency for your favourite test framework you want to use in your tests.\n    // TestNG is also supported by the Gradle Test task. Just change the\n    // testCompile dependency to testCompile 'org.testng:testng:6.8.1' and add\n    // 'test.useTestNG()' to your build script.\n    testCompile 'junit:junit:4.12'\n}\n"
  },
  {
    "path": "gradle/wrapper/gradle-wrapper.properties",
    "content": "#Wed Jul 13 08:15:29 MDT 2016\ndistributionBase=GRADLE_USER_HOME\ndistributionPath=wrapper/dists\nzipStoreBase=GRADLE_USER_HOME\nzipStorePath=wrapper/dists\ndistributionUrl=https\\://services.gradle.org/distributions/gradle-2.14-bin.zip\n"
  },
  {
    "path": "gradlew",
    "content": "#!/usr/bin/env bash\n\n##############################################################################\n##\n##  Gradle start up script for UN*X\n##\n##############################################################################\n\n# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.\nDEFAULT_JVM_OPTS=\"\"\n\nAPP_NAME=\"Gradle\"\nAPP_BASE_NAME=`basename \"$0\"`\n\n# Use the maximum available, or set MAX_FD != -1 to use that value.\nMAX_FD=\"maximum\"\n\nwarn ( ) {\n    echo \"$*\"\n}\n\ndie ( ) {\n    echo\n    echo \"$*\"\n    echo\n    exit 1\n}\n\n# OS specific support (must be 'true' or 'false').\ncygwin=false\nmsys=false\ndarwin=false\ncase \"`uname`\" in\n  CYGWIN* )\n    cygwin=true\n    ;;\n  Darwin* )\n    darwin=true\n    ;;\n  MINGW* )\n    msys=true\n    ;;\nesac\n\n# For Cygwin, ensure paths are in UNIX format before anything is touched.\nif $cygwin ; then\n    [ -n \"$JAVA_HOME\" ] && JAVA_HOME=`cygpath --unix \"$JAVA_HOME\"`\nfi\n\n# Attempt to set APP_HOME\n# Resolve links: $0 may be a link\nPRG=\"$0\"\n# Need this for relative symlinks.\nwhile [ -h \"$PRG\" ] ; do\n    ls=`ls -ld \"$PRG\"`\n    link=`expr \"$ls\" : '.*-> \\(.*\\)$'`\n    if expr \"$link\" : '/.*' > /dev/null; then\n        PRG=\"$link\"\n    else\n        PRG=`dirname \"$PRG\"`\"/$link\"\n    fi\ndone\nSAVED=\"`pwd`\"\ncd \"`dirname \\\"$PRG\\\"`/\" >&-\nAPP_HOME=\"`pwd -P`\"\ncd \"$SAVED\" >&-\n\nCLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar\n\n# Determine the Java command to use to start the JVM.\nif [ -n \"$JAVA_HOME\" ] ; then\n    if [ -x \"$JAVA_HOME/jre/sh/java\" ] ; then\n        # IBM's JDK on AIX uses strange locations for the executables\n        JAVACMD=\"$JAVA_HOME/jre/sh/java\"\n    else\n        JAVACMD=\"$JAVA_HOME/bin/java\"\n    fi\n    if [ ! -x \"$JAVACMD\" ] ; then\n        die \"ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME\n\nPlease set the JAVA_HOME variable in your environment to match the\nlocation of your Java installation.\"\n    fi\nelse\n    JAVACMD=\"java\"\n    which java >/dev/null 2>&1 || die \"ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.\n\nPlease set the JAVA_HOME variable in your environment to match the\nlocation of your Java installation.\"\nfi\n\n# Increase the maximum file descriptors if we can.\nif [ \"$cygwin\" = \"false\" -a \"$darwin\" = \"false\" ] ; then\n    MAX_FD_LIMIT=`ulimit -H -n`\n    if [ $? -eq 0 ] ; then\n        if [ \"$MAX_FD\" = \"maximum\" -o \"$MAX_FD\" = \"max\" ] ; then\n            MAX_FD=\"$MAX_FD_LIMIT\"\n        fi\n        ulimit -n $MAX_FD\n        if [ $? -ne 0 ] ; then\n            warn \"Could not set maximum file descriptor limit: $MAX_FD\"\n        fi\n    else\n        warn \"Could not query maximum file descriptor limit: $MAX_FD_LIMIT\"\n    fi\nfi\n\n# For Darwin, add options to specify how the application appears in the dock\nif $darwin; then\n    GRADLE_OPTS=\"$GRADLE_OPTS \\\"-Xdock:name=$APP_NAME\\\" \\\"-Xdock:icon=$APP_HOME/media/gradle.icns\\\"\"\nfi\n\n# For Cygwin, switch paths to Windows format before running java\nif $cygwin ; then\n    APP_HOME=`cygpath --path --mixed \"$APP_HOME\"`\n    CLASSPATH=`cygpath --path --mixed \"$CLASSPATH\"`\n\n    # We build the pattern for arguments to be converted via cygpath\n    ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null`\n    SEP=\"\"\n    for dir in $ROOTDIRSRAW ; do\n        ROOTDIRS=\"$ROOTDIRS$SEP$dir\"\n        SEP=\"|\"\n    done\n    OURCYGPATTERN=\"(^($ROOTDIRS))\"\n    # Add a user-defined pattern to the cygpath arguments\n    if [ \"$GRADLE_CYGPATTERN\" != \"\" ] ; then\n        OURCYGPATTERN=\"$OURCYGPATTERN|($GRADLE_CYGPATTERN)\"\n    fi\n    # Now convert the arguments - kludge to limit ourselves to /bin/sh\n    i=0\n    for arg in \"$@\" ; do\n        CHECK=`echo \"$arg\"|egrep -c \"$OURCYGPATTERN\" -`\n        CHECK2=`echo \"$arg\"|egrep -c \"^-\"`                                 ### Determine if an option\n\n        if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then                    ### Added a condition\n            eval `echo args$i`=`cygpath --path --ignore --mixed \"$arg\"`\n        else\n            eval `echo args$i`=\"\\\"$arg\\\"\"\n        fi\n        i=$((i+1))\n    done\n    case $i in\n        (0) set -- ;;\n        (1) set -- \"$args0\" ;;\n        (2) set -- \"$args0\" \"$args1\" ;;\n        (3) set -- \"$args0\" \"$args1\" \"$args2\" ;;\n        (4) set -- \"$args0\" \"$args1\" \"$args2\" \"$args3\" ;;\n        (5) set -- \"$args0\" \"$args1\" \"$args2\" \"$args3\" \"$args4\" ;;\n        (6) set -- \"$args0\" \"$args1\" \"$args2\" \"$args3\" \"$args4\" \"$args5\" ;;\n        (7) set -- \"$args0\" \"$args1\" \"$args2\" \"$args3\" \"$args4\" \"$args5\" \"$args6\" ;;\n        (8) set -- \"$args0\" \"$args1\" \"$args2\" \"$args3\" \"$args4\" \"$args5\" \"$args6\" \"$args7\" ;;\n        (9) set -- \"$args0\" \"$args1\" \"$args2\" \"$args3\" \"$args4\" \"$args5\" \"$args6\" \"$args7\" \"$args8\" ;;\n    esac\nfi\n\n# Split up the JVM_OPTS And GRADLE_OPTS values into an array, following the shell quoting and substitution rules\nfunction splitJvmOpts() {\n    JVM_OPTS=(\"$@\")\n}\neval splitJvmOpts $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS\nJVM_OPTS[${#JVM_OPTS[*]}]=\"-Dorg.gradle.appname=$APP_BASE_NAME\"\n\nexec \"$JAVACMD\" \"${JVM_OPTS[@]}\" -classpath \"$CLASSPATH\" org.gradle.wrapper.GradleWrapperMain \"$@\"\n"
  },
  {
    "path": "gradlew.bat",
    "content": "@if \"%DEBUG%\" == \"\" @echo off\r\n@rem ##########################################################################\r\n@rem\r\n@rem  Gradle startup script for Windows\r\n@rem\r\n@rem ##########################################################################\r\n\r\n@rem Set local scope for the variables with windows NT shell\r\nif \"%OS%\"==\"Windows_NT\" setlocal\r\n\r\n@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.\r\nset DEFAULT_JVM_OPTS=\r\n\r\nset DIRNAME=%~dp0\r\nif \"%DIRNAME%\" == \"\" set DIRNAME=.\r\nset APP_BASE_NAME=%~n0\r\nset APP_HOME=%DIRNAME%\r\n\r\n@rem Find java.exe\r\nif defined JAVA_HOME goto findJavaFromJavaHome\r\n\r\nset JAVA_EXE=java.exe\r\n%JAVA_EXE% -version >NUL 2>&1\r\nif \"%ERRORLEVEL%\" == \"0\" goto init\r\n\r\necho.\r\necho ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.\r\necho.\r\necho Please set the JAVA_HOME variable in your environment to match the\r\necho location of your Java installation.\r\n\r\ngoto fail\r\n\r\n:findJavaFromJavaHome\r\nset JAVA_HOME=%JAVA_HOME:\"=%\r\nset JAVA_EXE=%JAVA_HOME%/bin/java.exe\r\n\r\nif exist \"%JAVA_EXE%\" goto init\r\n\r\necho.\r\necho ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%\r\necho.\r\necho Please set the JAVA_HOME variable in your environment to match the\r\necho location of your Java installation.\r\n\r\ngoto fail\r\n\r\n:init\r\n@rem Get command-line arguments, handling Windowz variants\r\n\r\nif not \"%OS%\" == \"Windows_NT\" goto win9xME_args\r\nif \"%@eval[2+2]\" == \"4\" goto 4NT_args\r\n\r\n:win9xME_args\r\n@rem Slurp the command line arguments.\r\nset CMD_LINE_ARGS=\r\nset _SKIP=2\r\n\r\n:win9xME_args_slurp\r\nif \"x%~1\" == \"x\" goto execute\r\n\r\nset CMD_LINE_ARGS=%*\r\ngoto execute\r\n\r\n:4NT_args\r\n@rem Get arguments from the 4NT Shell from JP Software\r\nset CMD_LINE_ARGS=%$\r\n\r\n:execute\r\n@rem Setup the command line\r\n\r\nset CLASSPATH=%APP_HOME%\\gradle\\wrapper\\gradle-wrapper.jar\r\n\r\n@rem Execute Gradle\r\n\"%JAVA_EXE%\" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% \"-Dorg.gradle.appname=%APP_BASE_NAME%\" -classpath \"%CLASSPATH%\" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS%\r\n\r\n:end\r\n@rem End local scope for the variables with windows NT shell\r\nif \"%ERRORLEVEL%\"==\"0\" goto mainEnd\r\n\r\n:fail\r\nrem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of\r\nrem the _cmd.exe /c_ return code!\r\nif  not \"\" == \"%GRADLE_EXIT_CONSOLE%\" exit 1\r\nexit /b 1\r\n\r\n:mainEnd\r\nif \"%OS%\"==\"Windows_NT\" endlocal\r\n\r\n:omega\r\n"
  },
  {
    "path": "settings.gradle",
    "content": "/*\n * This settings file was auto generated by the Gradle buildInit task\n * by 'scott' at '10/12/15 12:35 PM' with Gradle 2.7\n *\n * The settings file is used to specify which projects to include in your build.\n * In a single project build this file can be empty or even removed.\n *\n * Detailed information about configuring a multi-project build in Gradle can be found\n * in the user guide at http://gradle.org/docs/2.7/userguide/multi_project_builds.html\n */\n\n/*\n// To declare projects as part of a multi-project build use the 'include' method\ninclude 'shared'\ninclude 'api'\ninclude 'services:webservice'\n*/\n\nrootProject.name = 'trilateration'\n"
  },
  {
    "path": "src/main/java/com/lemmingapex/trilateration/LinearLeastSquaresSolver.java",
    "content": "package com.lemmingapex.trilateration;\n\nimport org.apache.commons.math3.linear.*;\n\n/**\n *\n * For testing only. A linear approach to solve the Trilateration problem.\n * see http://inside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf\n *\n * @author scott\n */\npublic class LinearLeastSquaresSolver {\n\n    protected final TrilaterationFunction function;\n\n    public LinearLeastSquaresSolver(TrilaterationFunction function) {\n        this.function = function;\n    }\n\n    public RealVector solve(boolean debugInfo) {\n        int numberOfPositions = function.getPositions().length;\n        int positionDimension = function.getPositions()[0].length;\n\n        double[][] Ad = new double[numberOfPositions - 1][positionDimension];\n\n        // TODO: which reference position should be used?  currently using postion and distance in index 0.\n\n        for (int i = 1; i < numberOfPositions; i++) {\n            double[] Adi = new double[positionDimension];\n            for (int j = 0; j < positionDimension; j++) {\n                Adi[j] = function.getPositions()[i][j] - function.getPositions()[0][j];\n            }\n            Ad[i - 1] = Adi;\n        }\n        if (debugInfo) {\n            System.out.println(new Array2DRowRealMatrix(Ad));\n        }\n\n        // reference point is function.getPositions()[0], with distance function.getDistances()[0]\n        double referenceDistance = function.getDistances()[0];\n        double r0squared = referenceDistance * referenceDistance;\n        double[] bd = new double[numberOfPositions - 1];\n        for (int i = 1; i < numberOfPositions; i++) {\n            double ri = function.getDistances()[i];\n            double risquared = ri * ri;\n\n            // find distance between ri and r0\n            double di0squared = 0;\n            for (int j = 0; j < positionDimension; j++) {\n                double dij0j = function.getPositions()[i][j] - function.getPositions()[0][j];\n                di0squared += dij0j * dij0j;\n            }\n            bd[i - 1] = 0.5 * (r0squared - risquared + di0squared);\n        }\n        if (debugInfo) {\n            System.out.println(new ArrayRealVector(bd));\n        }\n\n        RealMatrix A = new Array2DRowRealMatrix(Ad, false);\n        RealVector b = new ArrayRealVector(bd, false);\n        DecompositionSolver solver = new QRDecomposition(A).getSolver();\n        RealVector x;\n        if(!solver.isNonSingular()) {\n            // bummer...\n            x = new ArrayRealVector(new double[positionDimension]);\n        } else {\n             x = solver.solve(b);\n        }\n\n        return x.add(new ArrayRealVector(function.getPositions()[0]));\n    }\n\n    public RealVector solve() {\n        return solve(false);\n    }\n}\n"
  },
  {
    "path": "src/main/java/com/lemmingapex/trilateration/NonLinearLeastSquaresSolver.java",
    "content": "package com.lemmingapex.trilateration;\n\nimport org.apache.commons.math3.fitting.leastsquares.LeastSquaresFactory;\nimport org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer;\nimport org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer.Optimum;\nimport org.apache.commons.math3.fitting.leastsquares.LeastSquaresProblem;\nimport org.apache.commons.math3.linear.ArrayRealVector;\nimport org.apache.commons.math3.linear.DiagonalMatrix;\n\n/**\n * Solves a Trilateration problem with an instance of a\n * {@link LeastSquaresOptimizer}\n *\n * @author scott\n *\n */\npublic class NonLinearLeastSquaresSolver {\n\n\tprotected final TrilaterationFunction function;\n\tprotected final LeastSquaresOptimizer leastSquaresOptimizer;\n\n\tprotected final static int MAXNUMBEROFITERATIONS = 1000;\n\n\tpublic NonLinearLeastSquaresSolver(TrilaterationFunction function, LeastSquaresOptimizer leastSquaresOptimizer) {\n\t\tthis.function = function;\n\t\tthis.leastSquaresOptimizer = leastSquaresOptimizer;\n\t}\n\n\tpublic Optimum solve(double[] target, double[] weights, double[] initialPoint, boolean debugInfo) {\n\t\tif (debugInfo) {\n\t\t\tSystem.out.println(\"Max Number of Iterations : \" + MAXNUMBEROFITERATIONS);\n\t\t}\n\n\t\tLeastSquaresProblem leastSquaresProblem = LeastSquaresFactory.create(\n\t\t\t\t// function to be optimized\n\t\t\t\tfunction,\n\t\t\t\t// target values at optimal point in least square equation\n\t\t\t\t// (x0+xi)^2 + (y0+yi)^2 + ri^2 = target[i]\n\t\t\t\tnew ArrayRealVector(target, false), new ArrayRealVector(initialPoint, false), new DiagonalMatrix(weights), null, MAXNUMBEROFITERATIONS, MAXNUMBEROFITERATIONS);\n\n\t\treturn leastSquaresOptimizer.optimize(leastSquaresProblem);\n\t}\n\n\tpublic Optimum solve(double[] target, double[] weights, double[] initialPoint) {\n\t\treturn solve(target, weights, initialPoint, false);\n\t}\n\n\tpublic Optimum solve(boolean debugInfo) {\n\t\tint numberOfPositions = function.getPositions().length;\n\t\tint positionDimension = function.getPositions()[0].length;\n\n\t\tdouble[] initialPoint = new double[positionDimension];\n\t\t// initial point, use average of the vertices\n\t\tfor (int i = 0; i < function.getPositions().length; i++) {\n\t\t\tdouble[] vertex = function.getPositions()[i];\n\t\t\tfor (int j = 0; j < vertex.length; j++) {\n\t\t\t\tinitialPoint[j] += vertex[j];\n\t\t\t}\n\t\t}\n\t\tfor (int j = 0; j < initialPoint.length; j++) {\n\t\t\tinitialPoint[j] /= numberOfPositions;\n\t\t}\n\n\t\tif (debugInfo) {\n\t\t\tStringBuilder output = new StringBuilder(\"initialPoint: \");\n\t\t\tfor (int i = 0; i < initialPoint.length; i++) {\n\t\t\t\toutput.append(initialPoint[i]).append(\" \");\n\t\t\t}\n\t\t\tSystem.out.println(output.toString());\n\t\t}\n\n\t\tdouble[] target = new double[numberOfPositions];\n\t\tdouble[] distances = function.getDistances();\n\t\tdouble[] weights = new double[target.length];\n\t\tfor (int i = 0; i < target.length; i++) {\n\t\t\ttarget[i] = 0.0;\n\t\t\tweights[i] = inverseSquareLaw(distances[i]);\n\t\t}\n\n\t\treturn solve(target, weights, initialPoint, debugInfo);\n\t}\n\n\tprivate double inverseSquareLaw(double distance) {\n\t\treturn 1 / (distance * distance);\n\t}\n\n\tpublic Optimum solve() {\n\t\treturn solve(false);\n\t}\n}\n"
  },
  {
    "path": "src/main/java/com/lemmingapex/trilateration/TrilaterationFunction.java",
    "content": "package com.lemmingapex.trilateration;\n\nimport org.apache.commons.math3.fitting.leastsquares.MultivariateJacobianFunction;\nimport org.apache.commons.math3.linear.Array2DRowRealMatrix;\nimport org.apache.commons.math3.linear.ArrayRealVector;\nimport org.apache.commons.math3.linear.RealMatrix;\nimport org.apache.commons.math3.linear.RealVector;\nimport org.apache.commons.math3.util.Pair;\n\n/**\n * Models the Trilateration problem. This is a formulation for a nonlinear least\n * squares optimizer.\n *\n * @author scott\n *\n */\npublic class TrilaterationFunction implements MultivariateJacobianFunction {\n\n\tprotected static final double epsilon = 1E-7;\n\n\t/**\n\t * Known positions of static nodes\n\t */\n\tprotected final double positions[][];\n\n\t/**\n\t * Euclidean distances from static nodes to mobile node\n\t */\n\tprotected final double distances[];\n\n\tpublic TrilaterationFunction(double positions[][], double distances[]) {\n\n\t\tif(positions.length < 2) {\n\t\t\tthrow new IllegalArgumentException(\"Need at least two positions.\");\n\t\t}\n\n\t\tif(positions.length != distances.length) {\n\t\t\tthrow new IllegalArgumentException(\"The number of positions you provided, \" + positions.length + \", does not match the number of distances, \" + distances.length + \".\");\n\t\t}\n\n\t\t// bound distances to strictly positive domain\n\t\tfor (int i = 0; i < distances.length; i++) {\n\t\t\tdistances[i] = Math.max(distances[i], epsilon);\n\t\t}\n\n\t\tint positionDimension = positions[0].length;\n\t\tfor (int i = 1; i < positions.length; i++) {\n\t\t\tif(positionDimension != positions[i].length) {\n\t\t\t\tthrow new IllegalArgumentException(\"The dimension of all positions should be the same.\");\n\t\t\t}\n\t\t}\n\n\t\tthis.positions = positions;\n\t\tthis.distances = distances;\n\t}\n\n\tpublic final double[] getDistances() {\n\t\treturn distances;\n\t}\n\n\tpublic final double[][] getPositions() {\n\t\treturn positions;\n\t}\n\n\t/**\n\t * Calculate and return Jacobian function Actually return initialized function\n\t *\n\t * Jacobian matrix, [i][j] at\n\t * J[i][0] = delta_[(x0-xi)^2 + (y0-yi)^2 - ri^2]/delta_[x0] at\n\t * 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\n\t *\n\t * @param point for which to calculate the slope\n\t * @return Jacobian matrix for point\n\t */\n\tpublic RealMatrix jacobian(RealVector point) {\n\t\tdouble[] pointArray = point.toArray();\n\n\t\tdouble[][] jacobian = new double[distances.length][pointArray.length];\n\t\tfor (int i = 0; i < jacobian.length; i++) {\n\t\t\tfor (int j = 0; j < pointArray.length; j++) {\n\t\t\t\tjacobian[i][j] = 2 * pointArray[j] - 2 * positions[i][j];\n\t\t\t}\n\t\t}\n\n\t\treturn new Array2DRowRealMatrix(jacobian);\n\t}\n\n\t@Override\n\tpublic Pair<RealVector, RealMatrix> value(RealVector point) {\n\n\t\t// input\n\t\tdouble[] pointArray = point.toArray();\n\n\t\t// output\n\t\tdouble[] resultPoint = new double[this.distances.length];\n\n\t\t// compute least squares\n\t\tfor (int i = 0; i < resultPoint.length; i++) {\n\t\t\tresultPoint[i] = 0.0;\n\t\t\t// calculate sum, add to overall\n\t\t\tfor (int j = 0; j < pointArray.length; j++) {\n\t\t\t\tresultPoint[i] += (pointArray[j] - this.getPositions()[i][j]) * (pointArray[j] - this.getPositions()[i][j]);\n\t\t\t}\n\t\t\tresultPoint[i] -= (this.getDistances()[i]) * (this.getDistances()[i]);\n\t\t}\n\n\t\tRealMatrix jacobian = jacobian(point);\n\t\treturn new Pair<RealVector, RealMatrix>(new ArrayRealVector(resultPoint), jacobian);\n\t}\n}\n"
  },
  {
    "path": "src/test/java/com/lemmingapex/trilateration/TrilaterationTest.java",
    "content": "package com.lemmingapex.trilateration;\n\nimport static org.junit.Assert.assertEquals;\n\nimport org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer.Optimum;\nimport org.apache.commons.math3.fitting.leastsquares.LevenbergMarquardtOptimizer;\nimport org.apache.commons.math3.linear.RealMatrix;\nimport org.apache.commons.math3.linear.RealVector;\nimport org.apache.commons.math3.linear.SingularMatrixException;\n\n/**\n * Test class which is initialized with different predefined test cases.\n * Test was refactored from @author scott\n *\n * @author burfi\n */\npublic class TrilaterationTest {\n\n\tprivate double[][] positions;\n\tprivate double[] distances;\n\tprivate double[] expectedPosition;\n\tprivate double acceptedDelta;\n\tprivate StringBuilder output;\n\n\tprivate RealVector linearCalculatedPosition;\n\tprivate Optimum nonLinearOptimum;\n\n\tpublic TrilaterationTest(double[][] positions, double[] distances, double[] expectedPosition, double acceptedDelta) {\n\t\tthis.positions = positions;\n\t\tthis.distances = distances;\n\t\tthis.expectedPosition = expectedPosition;\n\t\tthis.acceptedDelta = acceptedDelta;\n\t\ttestCase();\n\t\toutputResult();\n\t\tcompareExpectedAndCalculatedResults();\n\t}\n\n\tprivate void testCase() {\n\t\tTrilaterationFunction trilaterationFunction = new TrilaterationFunction(positions, distances);\n\t\tLinearLeastSquaresSolver lSolver = new LinearLeastSquaresSolver(trilaterationFunction);\n\t\tNonLinearLeastSquaresSolver nlSolver = new NonLinearLeastSquaresSolver(trilaterationFunction, new LevenbergMarquardtOptimizer());\n\n\t\tlinearCalculatedPosition = lSolver.solve();\n\t\tnonLinearOptimum = nlSolver.solve();\n\t}\n\n\tprivate void outputResult() {\n\t\toutput = new StringBuilder();\n\t\tprintDoubleArray(\"expectedPosition: \", expectedPosition);\n\t\tprintDoubleArray(\"linear calculatedPosition: \", linearCalculatedPosition.toArray());\n\t\tprintDoubleArray(\"non-linear calculatedPosition: \", nonLinearOptimum.getPoint().toArray());\n\t\toutput.append(\"numberOfIterations: \").append(nonLinearOptimum.getIterations()).append(\"\\n\");\n\t\toutput.append(\"numberOfEvaluations: \").append(nonLinearOptimum.getEvaluations()).append(\"\\n\");\n\t\ttry {\n\t\t\tRealVector standardDeviation = nonLinearOptimum.getSigma(0);\n\t\t\tprintDoubleArray(\"standardDeviation: \", standardDeviation.toArray());\n\t\t\toutput.append(\"Norm of deviation: \").append(standardDeviation.getNorm()).append(\"\\n\");\n\t\t\tRealMatrix covarianceMatrix = nonLinearOptimum.getCovariances(0);\n\t\t\toutput.append(\"covarianceMatrix: \").append(covarianceMatrix).append(\"\\n\");\n\t\t} catch (SingularMatrixException e) {\n\t\t\tSystem.err.println(e.getMessage());\n\t\t}\n\n\t\tSystem.out.println(output.toString());\n\t}\n\n\tprivate void compareExpectedAndCalculatedResults() {\n\t\tdouble[] calculatedPosition = nonLinearOptimum.getPoint().toArray();\n\t\tfor (int i = 0; i < calculatedPosition.length; i++) {\n\t\t\tassertEquals(expectedPosition[i], calculatedPosition[i], acceptedDelta);\n\t\t}\n\t}\n\n\tprivate void printDoubleArray(String tag, double[] values) {\n\t\toutput.append(tag);\n\t\tfor (double p : values) {\n\t\t\toutput.append(p).append(\" \");\n\t\t}\n\t\toutput.append(\"\\n\");\n\t}\n}"
  },
  {
    "path": "src/test/java/com/lemmingapex/trilateration/TrilaterationTestCases.java",
    "content": "package com.lemmingapex.trilateration;\n\nimport org.junit.Test;\n\n/**\n * Test class which is initialized with different predefined test cases.\n * All test cases were defined by @author scott\n *\n * @author burfi\n */\npublic class TrilaterationTestCases {\n\n    @Test\n    public void trilateration1DExact1() throws Exception {\n        double[][] positions = new double[][]{{1.0}, {2.0}, {3.0}};\n        double[] distances = new double[]{1.1, 0.1, 0.9};\n        double[] expectedPosition = new double[]{2.1};\n        double acceptedDelta = 0.0001;\n        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);\n    }\n\n    @Test\n    public void trilateration1DExact2() throws Exception {\n        double[][] positions = new double[][]{{1000.0}, {2000.0}, {3000.0}};\n        double[] distances = new double[]{1100, 100, 900};\n        double[] expectedPosition = new double[]{2100.0};\n        double acceptedDelta = 0.0001;\n        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);\n    }\n\n    @Test\n    public void trilateration1DInexact() throws Exception {\n        double[][] positions = new double[][]{{1000.0}, {2000.0}, {3000.0}};\n        double[] distances = new double[]{1110, 110, 910};\n        double[] expectedPosition = new double[]{2100.0};\n        double acceptedDelta = 30;\n        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);\n    }\n\n    @Test\n    public void trilateration2DExact1() throws Exception {\n        double[][] positions = new double[][]{{1.0, 1.0}, {3.0, 1.0}, {2.0, 2.0}};\n        double[] distances = new double[]{1.0, 1.0, 1.0};\n        double[] expectedPosition = new double[]{2.0, 1.0};\n        double acceptedDelta = 0.0001;\n        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);\n    }\n\n    @Test\n    public void trilateration2DZeroDistance() throws Exception {\n        double[][] positions = new double[][]{{1.0, 1.0}, {2.0, 1.0}};\n        double[] distances = new double[]{0.0, 1.0};\n        double[] expectedPosition = new double[]{1.0, 1.0};\n        double acceptedDelta = 0.0001;\n        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);\n    }\n\n    @Test\n    public void trilateration2DExact2() throws Exception {\n        double[][] positions = new double[][]{{0.0, 0.0}, {-1.0, 0.0}, {0.0, -1.0}};\n        double[] distances = new double[]{Math.sqrt(2.0), 1.0, 1.0};\n        double[] expectedPosition = new double[]{-1.0, -1.0};\n        double acceptedDelta = 0.0001;\n        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);\n    }\n\n    @Test\n    public void trilateration2DExact3() throws Exception {\n        double[][] positions = new double[][]{{0.0, 0.0}, {1000.0, 0.0}, {0.0, 1000.0}};\n        double[] distances = new double[]{Math.sqrt(2.0) * 1000.0, 1000.0, 1000.0};\n        double[] expectedPosition = new double[]{1000.0, 1000.0};\n        double acceptedDelta = 0.0001;\n        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);\n    }\n\n    @Test\n    public void trilateration2DExact4() throws Exception {\n        double[][] positions = new double[][]{{1.0, 1.0}, {1.0, 3.0}, {8.0, 8.0}, {2.0, 2.0}};\n        double[] distances = new double[]{5.0, 5.0, 6.36, 3.9};\n        double[] expectedPosition = new double[]{5.9, 2.0};\n        double acceptedDelta = 0.01;\n        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);\n    }\n\n    @Test\n    public void trilateration2DExact5() throws Exception {\n        double[][] positions = new double[][]{{5.0, -6.0}, {13.0, -15.0}, {21.0, -3.0}};\n        double[] distances = new double[]{8.06, 13.97, 23.32};\n        double[] expectedPosition = new double[]{-0.6, -11.8};\n        double acceptedDelta = 0.01;\n        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);\n    }\n\n    @Test\n    public void trilateration2DInexact1() throws Exception {\n        double[][] positions = new double[][]{{1.0, 1.0}, {3.0, 1.0}, {2.0, 2.0}};\n        double[] distances = new double[]{0.9, 1.0, 1.0};\n        double[] expectedPosition = new double[]{2.0, 1.0};\n        double acceptedDelta = 0.1;\n        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);\n    }\n\n    @Test\n    public void trilateration2DInexact2() throws Exception {\n        double[][] positions = new double[][]{{5.0, -6.0}, {13.0, -15.0}, {21.0, -3.0}, {12.42, -21.2}};\n        double[] distances = new double[]{8.06, 13.97, 23.32, 15.31};\n        double[] expectedPosition = new double[]{-0.6, -11.8};\n        double acceptedDelta = 1.0;\n        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);\n    }\n\n    @Test\n    public void trilateration2DNonIntersecting() throws Exception {\n        double[][] positions = new double[][]{{1.0, 1.0}, {3.0, 1.0}, {2.0, 2.0}};\n        double[] distances = new double[]{0.5, 0.5, 0.5};\n        double[] expectedPosition = new double[]{2.0, 1.0};\n        double acceptedDelta = 0.25;\n        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);\n    }\n\n    @Test\n    public void trilateration2DOverIntersecting() throws Exception {\n        double[][] positions = new double[][]{{1.0, 1.0}, {3.0, 1.0}, {2.0, 2.0}};\n        double[] distances = new double[]{2.0, 2.0, 2.0};\n        double[] expectedPosition = new double[]{2.0, 1.0};\n        double acceptedDelta = 2.0;\n        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);\n    }\n\n    @Test\n    public void trilateration2DDegenerateCase1() throws Exception {\n        double[][] positions = new double[][]{{1.0, 1.0}, {1.0, 1.0}, {3.0, 1.0}};\n        double[] distances = new double[]{1.0, 1.0, 1.0};\n        double[] expectedPosition = new double[]{2.0, 1.0};\n        double acceptedDelta = 0.5;\n        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);\n    }\n\n    @Test\n    public void trilateration2DDegenerateCase2() throws Exception {\n        double[][] positions = new double[][]{{1.0, 1.0}, {1.0, 1.0}, {1.0, 1.0}};\n        double[] distances = new double[]{1.0, 1.0, 1.0};\n        double[] expectedPosition = new double[]{1.0, 1.0};\n        double acceptedDelta = 0.5;\n        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);\n    }\n\n    @Test\n    public void trilateration2DUnderdertermined() throws Exception {\n        double[][] positions = new double[][]{{1.0, 1.0}, {3.0, 1.0}};\n        double[] distances = new double[]{1.0, 1.0};\n        double[] expectedPosition = new double[]{2.0, 1.0};\n        double acceptedDelta = 0.5;\n        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);\n    }\n\n    @Test\n    public void trilateration3DExact() throws Exception {\n        double[][] positions = new double[][]{{1.0, 1.0, 1.0}, {3.0, 1.0, 1.0}, {2.0, 2.0, 1.0}};\n        double[] distances = new double[]{1.0, 1.0, 1.0};\n        double[] expectedPosition = new double[]{2.0, 1.0, 1.0};\n        double acceptedDelta = 0.0001;\n        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);\n    }\n\n    @Test\n    public void trilateration3DInexact() throws Exception {\n        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}};\n        double[] distances = new double[]{8.84, 8.84, 8.84, 8.84};\n        double[] expectedPosition = new double[]{5.2, -1.2, 7.7};\n        double acceptedDelta = 1.0;\n        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);\n    }\n\n    @Test\n    public void trilateration4DInexact() throws Exception {\n        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}};\n        double[] distances = new double[]{8.84, 8.84, 8.84, 8.84};\n        double[] expectedPosition = new double[]{5.2, -1.5, 7.7, 5.9};\n        double acceptedDelta = 1.0;\n        new TrilaterationTest(positions, distances, expectedPosition, acceptedDelta);\n    }\n}"
  }
]