NodeLocations.java

package iocomms.subpos;

/* Android API for SubPos (http://www.subpos.org)
 * Copyright (C) 2015 Blair Wyatt
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

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

import java.util.ArrayList;
import java.util.Date;


class ArrayLocations extends ArrayList<SPSData>
{
    ArrayList<SPSData> locations = new ArrayList<SPSData>();
    public ArrayLocations()
    {
        super();
    }

    @Override
    public int size()
    {
        return locations.size();
    }

    @Override
    public SPSData get(int index)
    {
        return locations.get(index);
    }

    @Override
    public boolean add(SPSData x)
    {
        SPSData temp;
        boolean averaged = false;
        //Find node that already exists
        for(int i=0; i<this.locations.size(); i++) {
            if (x.compareTo(this.locations.get(i)) == 1) {
                temp = this.locations.get(i);
                if (temp.num_of_averages > 1) {
                    temp.updateAverage(x.distance, x.rx_pwr);
                    averaged = true;
                } else {
                    this.locations.remove(x);
                }
                break;
            }
        }
        if (!averaged) {
            this.locations.add(x);
        }

        return true;
    }

}
public class NodeLocations {

    private  int averages = 0; //number of averages for node distance calcs.

    private ArrayLocations locations = new ArrayLocations();
    Position currentNodePosition = null;
    Position currentPosition = null;
    private boolean calculated = false;

    public ArrayList<SPSData> getLocations()
    {
        return locations.locations;
    }

    public void addLocation(char[] ssid, int rx_pwr, String bssid, int freq, long application_id,
                            boolean offset_mapping, boolean three_d_mapping)
    {
        SPSData location = new SPSData(averages);
        location.decodeSSID(ssid, rx_pwr, freq, bssid);
        if (location.app_id == application_id && location.off_map == offset_mapping &&
                location.three_d_map == three_d_mapping) {
            locations.add(location);
            calculated = false;
        }
    }
    public void setRollingAverage(int averages)
    {
        this.averages = averages;
    }

    public int size()
    {
        return locations.size();
    }

    public boolean isCalculated()
    {
        return calculated;
    }

    public void killAgedNodes(int secondsAge)
    {
        int i = 0;

        while (i < this.locations.size()) {
            if (((new Date()).getTime() - this.locations.get(i).seen.getTime())/1000 > secondsAge ) {
                this.locations.locations.remove(i);
            } else {
                i++;
            }
        }
    }


    @Override
    public String toString()
    {
        String build = new String();
        if (currentNodePosition != null)
        {
            build = currentNodePosition.lat + ", " + currentNodePosition.lng + ", "
                    + (currentNodePosition.altitude / 100); //Convert alt to meters
        }
        return build;
    }

    public void calculatePosition() {
        if (locations.size() > 0) {
            if (locations.size() == 1)
            {
                currentPosition = new Position(locations.get(0).lat, locations.get(0).lng,
                        locations.get(0).altitude, locations.get(0).distance);
            }
            else
            {
                currentPosition = trilaterate(locations);
            }
            calculated = true;
        }
    }

    //Cannot debug a single node
    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);

    }

}