n0tst3v3andClaude Opus 5
5d98f474ae
The pin sat dead ahead of the heading arrow no matter which way the user turned, so the arrow looked like it was aiming at the item and the item looked like it was always straight down the path of travel. The cause is geometric, not a UI link. addSample() only accepts a sample after 5 m of movement, so every sample lies on the line the user walked. gradientBearing() correlates RSSI against centred position, and on a collinear track each centred position is t*u for a single unit vector u along the walk - so the sum is exactly parallel to u regardless of the signal. The "estimated" bearing was the direction of travel, echoed back. That bearing then placed the pin in three separate paths: preliminaryEstimate() projects along it, solve() overrides a converged trilateration with it, and consensusEstimate() averages the result. All three produced a pin straight ahead. Refuse the bearing until the track has real width across its own axis - the smaller eigenvalue of the position covariance, against max(4 m, meanAccuracy/2). A straight walk with GPS jitter measures about 1 m of spread and is rejected; an L of two 20 m legs measures about 5 m and is accepted. Distances still place the item off-axis on a straight walk, but which side is a real mirror ambiguity, so the UI now says so and asks for a leg at 90 degrees instead of inventing a side. Also drops showFallbackPin(), which projected the pin along the raw compass heading whenever no estimate was solved - a fourth route to the same wrong place. With no estimate the pin now shows the item's last known GPS fix, or nothing at all. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
313 lines
15 KiB
Java
313 lines
15 KiB
Java
package com.wytehat.btlogger;
|
|
|
|
import java.util.ArrayList;
|
|
import java.util.List;
|
|
|
|
public class SpatialGradientEngine {
|
|
|
|
/**
|
|
* Metres of spread the sample track needs across its own axis before a
|
|
* signal gradient means anything. See {@link #gradientBearing}.
|
|
*/
|
|
private static final double MIN_TRACK_WIDTH_METERS = 4.0;
|
|
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* A bearing solved from samples that all sit on one line is not a
|
|
* measurement, it is an echo of the walk.
|
|
*
|
|
* Centre the positions and the correlation below is a sum of
|
|
* (position - mean) * signal. On a straight walk every centred position
|
|
* is t*u for one unit vector u along the track, so the result is exactly
|
|
* parallel to u no matter what the RSSI does - the "estimated" bearing
|
|
* comes back as the direction the user is already walking, the pin gets
|
|
* projected dead ahead, and the compass arrow appears to point at it.
|
|
*
|
|
* Distances alone still place the item off-axis, but which side of the
|
|
* track it lies on is a genuine mirror ambiguity. It takes a leg at an
|
|
* angle to resolve, so report NaN until the track has real width and let
|
|
* the caller ask the user to turn.
|
|
*/
|
|
private float gradientBearing(double[] xs, double[] ys) {
|
|
double meanX = 0, meanY = 0, meanRssi = 0, meanAccuracy = 0;
|
|
for (int i = 0; i < samples.size(); i++) {
|
|
meanX += xs[i]; meanY += ys[i]; meanRssi += samples.get(i).rssi;
|
|
meanAccuracy += samples.get(i).accuracy;
|
|
}
|
|
meanX /= samples.size(); meanY /= samples.size(); meanRssi /= samples.size();
|
|
meanAccuracy /= samples.size();
|
|
if (perpendicularSpread(xs, ys, meanX, meanY) <
|
|
Math.max(MIN_TRACK_WIDTH_METERS, meanAccuracy * 0.5)) {
|
|
return Float.NaN;
|
|
}
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* Spread of the sample track across its own dominant axis, in metres:
|
|
* the smaller eigenvalue of the position covariance, square-rooted.
|
|
*
|
|
* Straight walk plus GPS jitter lands around 1-3 m. An L of two 20 m
|
|
* legs lands near 6 m.
|
|
*/
|
|
private double perpendicularSpread(double[] xs, double[] ys,
|
|
double meanX, double meanY) {
|
|
int count = samples.size();
|
|
if (count < 3) return 0;
|
|
double sxx = 0, syy = 0, sxy = 0;
|
|
for (int i = 0; i < count; i++) {
|
|
double dx = xs[i] - meanX, dy = ys[i] - meanY;
|
|
sxx += dx * dx; syy += dy * dy; sxy += dx * dy;
|
|
}
|
|
sxx /= count; syy /= count; sxy /= count;
|
|
double half = (sxx + syy) / 2.0;
|
|
double gap = Math.sqrt(Math.pow((sxx - syy) / 2.0, 2) + sxy * sxy);
|
|
return Math.sqrt(Math.max(0, half - gap));
|
|
}
|
|
|
|
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));
|
|
}
|
|
}
|