Initial commit
This commit is contained in:
@@ -0,0 +1,261 @@
|
||||
package com.wytehat.btlogger;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
public class SpatialGradientEngine {
|
||||
public static class Estimate {
|
||||
public double latitude;
|
||||
public double longitude;
|
||||
public double confidenceMeters;
|
||||
public float gradientBearing;
|
||||
public int sampleCount;
|
||||
public boolean preliminary;
|
||||
}
|
||||
|
||||
private static class Sample {
|
||||
double latitude, longitude, rssi, distance;
|
||||
float accuracy;
|
||||
long time;
|
||||
}
|
||||
|
||||
private final ArrayList<Sample> samples = new ArrayList<Sample>();
|
||||
private final ArrayList<Double> pendingRssi = new ArrayList<Double>();
|
||||
private double pendingLatitude, pendingLongitude;
|
||||
private int lastConsensusSampleCount = -1;
|
||||
private final ArrayList<Estimate> estimateConsensus = new ArrayList<Estimate>();
|
||||
private Estimate stableEstimate;
|
||||
|
||||
public synchronized boolean addSample(double latitude, double longitude, float accuracy,
|
||||
double filteredRssi, int txPower,
|
||||
double pathLossExponent, long time) {
|
||||
if (accuracy <= 0 || accuracy > 60 || pathLossExponent <= 0) return false;
|
||||
if (samples.size() > 0) {
|
||||
Sample previous = samples.get(samples.size() - 1);
|
||||
double required = Math.max(5.0, Math.max(previous.accuracy, accuracy));
|
||||
if (haversine(previous.latitude, previous.longitude, latitude, longitude) < required) {
|
||||
pendingRssi.clear(); return false;
|
||||
}
|
||||
}
|
||||
if (pendingRssi.isEmpty()) {
|
||||
pendingLatitude = latitude; pendingLongitude = longitude;
|
||||
} else if (haversine(pendingLatitude, pendingLongitude, latitude, longitude) >
|
||||
Math.max(5.0, accuracy)) {
|
||||
pendingRssi.clear(); pendingLatitude = latitude; pendingLongitude = longitude;
|
||||
}
|
||||
pendingRssi.add(Double.valueOf(filteredRssi));
|
||||
if (pendingRssi.size() < 4) return false;
|
||||
java.util.Collections.sort(pendingRssi);
|
||||
double median = (pendingRssi.get(1).doubleValue() + pendingRssi.get(2).doubleValue()) / 2.0;
|
||||
pendingRssi.clear();
|
||||
Sample point = new Sample();
|
||||
point.latitude = latitude; point.longitude = longitude; point.accuracy = accuracy;
|
||||
point.rssi = median;
|
||||
point.distance = Math.pow(10.0, (txPower - median) / (10.0 * pathLossExponent));
|
||||
point.time = time; samples.add(point);
|
||||
while (samples.size() > 60) samples.remove(0);
|
||||
prune(time - 15L * 60L * 1000L);
|
||||
return true;
|
||||
}
|
||||
|
||||
public synchronized Estimate solve() {
|
||||
if (samples.size() < 4) return null;
|
||||
Sample origin = samples.get(0);
|
||||
int count = samples.size();
|
||||
double[] xs = new double[count];
|
||||
double[] ys = new double[count];
|
||||
double cos = Math.cos(Math.toRadians(origin.latitude));
|
||||
double metersPerDegree = 111319.49;
|
||||
double x = 0, y = 0, total = 0;
|
||||
for (int i = 0; i < count; i++) {
|
||||
Sample sample = samples.get(i);
|
||||
xs[i] = (sample.longitude - origin.longitude) * metersPerDegree * cos;
|
||||
ys[i] = (sample.latitude - origin.latitude) * metersPerDegree;
|
||||
double weight = 1.0 / Math.max(1.0, sample.distance * sample.distance);
|
||||
x += xs[i] * weight;
|
||||
y += ys[i] * weight;
|
||||
total += weight;
|
||||
}
|
||||
double maxBaseline = 0;
|
||||
for (int i = 0; i < count; i++) for (int j = i + 1; j < count; j++)
|
||||
maxBaseline = Math.max(maxBaseline, Math.sqrt(Math.pow(xs[i] - xs[j], 2) + Math.pow(ys[i] - ys[j], 2)));
|
||||
if (maxBaseline < 8.0) return null;
|
||||
x /= total;
|
||||
y /= total;
|
||||
|
||||
double determinant = 0;
|
||||
for (int iteration = 0; iteration < 20; iteration++) {
|
||||
double a00 = 0, a01 = 0, a11 = 0, b0 = 0, b1 = 0;
|
||||
for (int i = 0; i < count; i++) {
|
||||
Sample sample = samples.get(i);
|
||||
double dx = x - xs[i];
|
||||
double dy = y - ys[i];
|
||||
double radius = Math.max(0.25, Math.sqrt(dx * dx + dy * dy));
|
||||
double residual = radius - sample.distance;
|
||||
double jx = dx / radius;
|
||||
double jy = dy / radius;
|
||||
double uncertainty = sample.accuracy * sample.accuracy +
|
||||
Math.pow(Math.max(1.0, sample.distance * 0.5), 2);
|
||||
double weight = 1.0 / uncertainty;
|
||||
double scale = Math.sqrt(uncertainty);
|
||||
if (Math.abs(residual) > 2.5 * scale)
|
||||
weight *= (2.5 * scale) / Math.abs(residual);
|
||||
a00 += weight * jx * jx;
|
||||
a01 += weight * jx * jy;
|
||||
a11 += weight * jy * jy;
|
||||
b0 += weight * jx * residual;
|
||||
b1 += weight * jy * residual;
|
||||
}
|
||||
determinant = a00 * a11 - a01 * a01;
|
||||
if (Math.abs(determinant) < 0.000001) return null;
|
||||
double stepX = -(a11 * b0 - a01 * b1) / determinant;
|
||||
double stepY = -(-a01 * b0 + a00 * b1) / determinant;
|
||||
x += stepX;
|
||||
y += stepY;
|
||||
if (Math.sqrt(stepX * stepX + stepY * stepY) < 0.05) break;
|
||||
}
|
||||
|
||||
double error = 0;
|
||||
double accuracyTotal = 0;
|
||||
for (int i = 0; i < count; i++) {
|
||||
double radius = Math.sqrt(Math.pow(x - xs[i], 2) + Math.pow(y - ys[i], 2));
|
||||
error += Math.pow(radius - samples.get(i).distance, 2);
|
||||
accuracyTotal += samples.get(i).accuracy;
|
||||
}
|
||||
Estimate result = new Estimate();
|
||||
result.latitude = origin.latitude + y / metersPerDegree;
|
||||
result.longitude = origin.longitude + x / (metersPerDegree * cos);
|
||||
double averageAccuracy = accuracyTotal / count;
|
||||
result.confidenceMeters = Math.max(averageAccuracy * 0.75,
|
||||
Math.sqrt(error / count) + averageAccuracy / Math.sqrt(Math.max(1.0, count / 3.0)));
|
||||
result.gradientBearing = gradientBearing(xs, ys);
|
||||
Sample latest = samples.get(count - 1);
|
||||
double latestX = xs[count - 1], latestY = ys[count - 1];
|
||||
double solvedGap = Math.sqrt(Math.pow(x - latestX, 2) + Math.pow(y - latestY, 2));
|
||||
double gradientEast = Math.sin(Math.toRadians(result.gradientBearing));
|
||||
double gradientNorth = Math.cos(Math.toRadians(result.gradientBearing));
|
||||
double solvedEast = x - latestX;
|
||||
double solvedNorth = y - latestY;
|
||||
boolean oppositeGradient = solvedEast * gradientEast + solvedNorth * gradientNorth <= 0;
|
||||
if (!Float.isNaN(result.gradientBearing) && latest.distance > 3.0 &&
|
||||
(oppositeGradient || solvedGap < Math.max(3.0, latest.distance * 0.5))) {
|
||||
double angle = Math.toRadians(result.gradientBearing);
|
||||
double adjustedX = latestX + Math.sin(angle) * latest.distance;
|
||||
double adjustedY = latestY + Math.cos(angle) * latest.distance;
|
||||
result.latitude = origin.latitude + adjustedY / metersPerDegree;
|
||||
result.longitude = origin.longitude + adjustedX / (metersPerDegree * cos);
|
||||
}
|
||||
result.sampleCount = count;
|
||||
result.preliminary = false;
|
||||
return result;
|
||||
}
|
||||
|
||||
private float gradientBearing(double[] xs, double[] ys) {
|
||||
double meanX = 0, meanY = 0, meanRssi = 0;
|
||||
for (int i = 0; i < samples.size(); i++) {
|
||||
meanX += xs[i]; meanY += ys[i]; meanRssi += samples.get(i).rssi;
|
||||
}
|
||||
meanX /= samples.size(); meanY /= samples.size(); meanRssi /= samples.size();
|
||||
double east = 0, north = 0;
|
||||
for (int i = 0; i < samples.size(); i++) {
|
||||
double signal = samples.get(i).rssi - meanRssi;
|
||||
east += (xs[i] - meanX) * signal;
|
||||
north += (ys[i] - meanY) * signal;
|
||||
}
|
||||
if (Math.abs(east) + Math.abs(north) < 0.000001) return Float.NaN;
|
||||
double degrees = Math.toDegrees(Math.atan2(east, north));
|
||||
if (degrees < 0) degrees += 360;
|
||||
return (float) degrees;
|
||||
}
|
||||
|
||||
public synchronized Estimate preliminaryEstimate() {
|
||||
if (samples.size() < 2) return null;
|
||||
Sample origin = samples.get(0);
|
||||
int count = samples.size();
|
||||
double[] xs = new double[count]; double[] ys = new double[count];
|
||||
double cos = Math.cos(Math.toRadians(origin.latitude));
|
||||
double metersPerDegree = 111319.49;
|
||||
for (int i = 0; i < count; i++) {
|
||||
xs[i] = (samples.get(i).longitude - origin.longitude) * metersPerDegree * cos;
|
||||
ys[i] = (samples.get(i).latitude - origin.latitude) * metersPerDegree;
|
||||
}
|
||||
float bearing = gradientBearing(xs, ys);
|
||||
if (Float.isNaN(bearing)) return null;
|
||||
Sample latest = samples.get(count - 1);
|
||||
double projected = Math.max(5.0, Math.min(50.0, latest.distance));
|
||||
double radians = Math.toRadians(bearing);
|
||||
double north = Math.cos(radians) * projected;
|
||||
double east = Math.sin(radians) * projected;
|
||||
Estimate result = new Estimate();
|
||||
result.latitude = latest.latitude + north / metersPerDegree;
|
||||
result.longitude = latest.longitude + east / (metersPerDegree * Math.cos(Math.toRadians(latest.latitude)));
|
||||
result.confidenceMeters = Math.max(latest.accuracy, projected * 1.5);
|
||||
result.gradientBearing = bearing; result.sampleCount = count; result.preliminary = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
public synchronized Estimate consensusEstimate() {
|
||||
if (samples.size() == lastConsensusSampleCount) return stableEstimate;
|
||||
lastConsensusSampleCount = samples.size();
|
||||
Estimate candidate = solve();
|
||||
if (candidate == null) candidate = preliminaryEstimate();
|
||||
if (candidate == null) return stableEstimate;
|
||||
if (!estimateConsensus.isEmpty()) {
|
||||
Estimate previous = estimateConsensus.get(estimateConsensus.size() - 1);
|
||||
double separation = haversine(previous.latitude, previous.longitude,
|
||||
candidate.latitude, candidate.longitude);
|
||||
double allowed = Math.max(20.0, previous.confidenceMeters + candidate.confidenceMeters);
|
||||
if (separation > allowed) estimateConsensus.clear();
|
||||
}
|
||||
estimateConsensus.add(candidate);
|
||||
while (estimateConsensus.size() > 5) estimateConsensus.remove(0);
|
||||
if (estimateConsensus.size() < 3) return stableEstimate;
|
||||
int start = estimateConsensus.size() - 3;
|
||||
Estimate result = new Estimate(); double east = 0, north = 0;
|
||||
for (int i = start; i < estimateConsensus.size(); i++) {
|
||||
Estimate value = estimateConsensus.get(i);
|
||||
result.latitude += value.latitude; result.longitude += value.longitude;
|
||||
result.confidenceMeters += value.confidenceMeters;
|
||||
east += Math.sin(Math.toRadians(value.gradientBearing));
|
||||
north += Math.cos(Math.toRadians(value.gradientBearing));
|
||||
result.preliminary = result.preliminary || value.preliminary;
|
||||
}
|
||||
result.latitude /= 3.0; result.longitude /= 3.0;
|
||||
result.confidenceMeters = Math.max(2.0, result.confidenceMeters / 3.0);
|
||||
double bearing = Math.toDegrees(Math.atan2(east, north));
|
||||
if (bearing < 0) bearing += 360; result.gradientBearing = (float)bearing;
|
||||
result.sampleCount = samples.size(); stableEstimate = result; return stableEstimate;
|
||||
}
|
||||
|
||||
public synchronized double calibratePathLoss(double targetLat, double targetLon, int txPower) {
|
||||
if (samples.size() < 3) return Double.NaN;
|
||||
double numerator = 0, denominator = 0; int used = 0;
|
||||
for (int i = 0; i < samples.size(); i++) {
|
||||
Sample sample = samples.get(i);
|
||||
double distance = haversine(sample.latitude, sample.longitude, targetLat, targetLon);
|
||||
if (distance < 1.0 || distance > 100.0) continue;
|
||||
double x = 10.0 * Math.log10(distance);
|
||||
numerator += x * (txPower - sample.rssi); denominator += x * x; used++;
|
||||
}
|
||||
if (used < 3 || denominator <= 0) return Double.NaN;
|
||||
double exponent = numerator / denominator;
|
||||
return exponent >= 1.2 && exponent <= 5.0 ? exponent : Double.NaN;
|
||||
}
|
||||
|
||||
private void prune(long oldest) {
|
||||
while (samples.size() > 0 && samples.get(0).time < oldest) samples.remove(0);
|
||||
}
|
||||
|
||||
public synchronized int size() { return samples.size(); }
|
||||
public synchronized void clear() { samples.clear(); pendingRssi.clear(); estimateConsensus.clear(); stableEstimate = null; lastConsensusSampleCount = -1; }
|
||||
|
||||
private double haversine(double lat1, double lon1, double lat2, double lon2) {
|
||||
double radius = 6371000.0;
|
||||
double dLat = Math.toRadians(lat2 - lat1);
|
||||
double dLon = Math.toRadians(lon2 - lon1);
|
||||
double a = Math.sin(dLat / 2) * Math.sin(dLat / 2) +
|
||||
Math.cos(Math.toRadians(lat1)) * Math.cos(Math.toRadians(lat2)) *
|
||||
Math.sin(dLon / 2) * Math.sin(dLon / 2);
|
||||
return radius * 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user