From 787183277b89b76fc802eff32342b1e15d7b2d5a Mon Sep 17 00:00:00 2001 From: Junyi Lin Date: Fri, 20 Mar 2026 20:36:31 +0000 Subject: [PATCH 1/7] - Created a new `fusion` package to encapsulate all positioning algorithms. - Implemented `Particle` class with movement and noise simulation. - Implemented the `ParticleFilter` algorithm with the complete lifecycle: 1. Initialization (`initialize`): Distributed particles randomly within given map bounds. 2. Prediction (`predict`): Shifted particles based on PDR movement deltas. 3. Update (`updateWeights`): Calculated particle weights using Gaussian likelihood based on measurement distance and accuracy. 4. Resampling (`resample`): Implemented Low Variance Resampling (Roulette Wheel) to duplicate high-weight particles and eliminate low-weight ones. - Added data structure classes: `MapBounds`, `PDRMovement`, `Measurement`, and `Position`. - Added `getEstimatedPosition` using weighted average for robust position estimation. --- .../PositionMe/fusion/MapBounds.java | 15 ++ .../PositionMe/fusion/Measurement.java | 18 ++ .../PositionMe/fusion/PDRMovement.java | 15 ++ .../PositionMe/fusion/Particle.java | 36 +++ .../PositionMe/fusion/ParticleFilter.java | 211 ++++++++++++++++++ .../PositionMe/fusion/Position.java | 11 + .../PositionMe/sensors/SensorFusion.java | 14 ++ secrets.properties | 6 +- 8 files changed, 323 insertions(+), 3 deletions(-) create mode 100644 app/src/main/java/com/openpositioning/PositionMe/fusion/MapBounds.java create mode 100644 app/src/main/java/com/openpositioning/PositionMe/fusion/Measurement.java create mode 100644 app/src/main/java/com/openpositioning/PositionMe/fusion/PDRMovement.java create mode 100644 app/src/main/java/com/openpositioning/PositionMe/fusion/Particle.java create mode 100644 app/src/main/java/com/openpositioning/PositionMe/fusion/ParticleFilter.java create mode 100644 app/src/main/java/com/openpositioning/PositionMe/fusion/Position.java diff --git a/app/src/main/java/com/openpositioning/PositionMe/fusion/MapBounds.java b/app/src/main/java/com/openpositioning/PositionMe/fusion/MapBounds.java new file mode 100644 index 00000000..b0758df4 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/fusion/MapBounds.java @@ -0,0 +1,15 @@ +package com.openpositioning.PositionMe.fusion; + +public class MapBounds { + public final float minX; + public final float maxX; + public final float minY; + public final float maxY; + + public MapBounds(float minX, float maxX, float minY, float maxY) { + this.minX = minX; + this.maxX = maxX; + this.minY = minY; + this.maxY = maxY; + } +} diff --git a/app/src/main/java/com/openpositioning/PositionMe/fusion/Measurement.java b/app/src/main/java/com/openpositioning/PositionMe/fusion/Measurement.java new file mode 100644 index 00000000..c318607e --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/fusion/Measurement.java @@ -0,0 +1,18 @@ +package com.openpositioning.PositionMe.fusion; + +/** + * 这个类封装了一次外部测量的数据。 + * 来自Wi-Fi定位或GNSS定位。 + * 它包含一个坐标点和一个表示精度的值。 + */ +public class Measurement { + public final float x; + public final float y; + public final double accuracy; // 定位的精度/标准差（比如5米） + + public Measurement(float x, float y, double accuracy) { + this.x = x; + this.y = y; + this.accuracy = accuracy; + } +} diff --git a/app/src/main/java/com/openpositioning/PositionMe/fusion/PDRMovement.java b/app/src/main/java/com/openpositioning/PositionMe/fusion/PDRMovement.java new file mode 100644 index 00000000..82889c81 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/fusion/PDRMovement.java @@ -0,0 +1,15 @@ +package com.openpositioning.PositionMe.fusion; + +/** + * 这个类用来封装一次PDR（行人航位推算）的移动数据。 + * x方向和y方向的位移。 + */ +public class PDRMovement { + public final float deltaX; + public final float deltaY; + + public PDRMovement(float deltaX, float deltaY) { + this.deltaX = deltaX; + this.deltaY = deltaY; + } +} diff --git a/app/src/main/java/com/openpositioning/PositionMe/fusion/Particle.java b/app/src/main/java/com/openpositioning/PositionMe/fusion/Particle.java new file mode 100644 index 00000000..e37f068d --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/fusion/Particle.java @@ -0,0 +1,36 @@ +package com.openpositioning.PositionMe.fusion; + +import java.util.Random; +public class Particle { + float x; // x坐标 + float y; // y坐标 + int floor; // 楼层 + double weight; // 权重 + + // 构造函数等... + public Particle(float x, float y, int floor) { + this.x = x; + this.y = y; + this.floor = floor; + this.weight = 1.0; // 初始权重都一样 + } + + /** + * 根据给定的位移来移动粒子，并增加一些随机噪声。 + * @param deltaX X方向的位移 + * @param deltaY Y方向的位移 + * @param random 一个随机数生成器实例 + */ + public void move(float deltaX, float deltaY, Random random) { + // 这是基础移动：直接把位移加上去 + this.x += deltaX; + this.y += deltaY; + + // 这是关键的“增加噪声”部分 + // random.nextGaussian() 生成一个符合标准正态分布（均值为0，标准差为1）的随机数 + // 我们把它乘以一个很小的值（比如0.1），来控制噪声的大小（即标准差为0.1米） + // 这意味着大部分噪声会在-0.1米到+0.1米之间，少数会更大一些。这比均匀分布更真实。 + this.x += random.nextGaussian() * 0.1; + this.y += random.nextGaussian() * 0.1; + } +} diff --git a/app/src/main/java/com/openpositioning/PositionMe/fusion/ParticleFilter.java b/app/src/main/java/com/openpositioning/PositionMe/fusion/ParticleFilter.java new file mode 100644 index 00000000..0f68d07a --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/fusion/ParticleFilter.java @@ -0,0 +1,211 @@ +package com.openpositioning.PositionMe.fusion; + +import java.util.ArrayList; +import java.util.List; +import java.util.Random; + +public class ParticleFilter { + + private List particles; + private int numParticles; + private Random random = new Random(); // 在类的成员变量里创建一个随机数生成器 + + public ParticleFilter(int numParticles) { + this.numParticles = numParticles; + this.particles = new ArrayList<>(numParticles); + } + + /** + * 步骤 1: 初始化 + * 在地图的某个区域内随机撒点 + */ + /** + * 步骤 1: 初始化 + * 在地图的某个区域内随机撒点 + * @param mapBounds 地图的边界信息 + */ + public void initialize(MapBounds mapBounds) { + // 1. 清空旧的粒子列表，确保每次初始化都是全新的开始 + particles.clear(); + + // 2. 计算地图的宽度和高度 + float width = mapBounds.maxX - mapBounds.minX; + float height = mapBounds.maxY - mapBounds.minY; + + // 3. 这是一个循环，会执行 numParticles 次，i会从0变到numParticles-1 + for (int i = 0; i < numParticles; i++) { + + // 4. 生成一个0.0到1.0之间的随机小数，乘以宽度，再加上最小X值 + // 这就得到了一个在 minX 和 maxX 之间的随机X坐标 + float randomX = mapBounds.minX + random.nextFloat() * width; + + // 5. 同理，生成一个在 minY 和 maxY 之间的随机Y坐标 + float randomY = mapBounds.minY + random.nextFloat() * height; + + // 6. 假设开始时都在0楼 + int initialFloor = 0; + + // 7. 使用这些随机生成的坐标和楼层，创建一个新的粒子对象 + Particle newParticle = new Particle(randomX, randomY, initialFloor); + + // 8. 把这个新创建的粒子添加到我们的粒子列表中 + particles.add(newParticle); + } + } + + + /** + * 步骤 2: 预测 + * 根据 PDR 的移动数据，移动所有粒子 + * @param movement 包含本次移动 deltaX 和 deltaY 的对象 + */ + public void predict(PDRMovement movement) { + // 1. 获取本次移动的x和y方向的距离 + float dx = movement.deltaX; + float dy = movement.deltaY; + + // 2. 遍历粒子列表中的每一个粒子 p + for (Particle p : particles) { + // 3. 调用该粒子自己的 move 方法，让它移动 + // 我们把移动距离和粒子滤波器自己的随机数生成器传给它 + p.move(dx, dy, this.random); + } + } + + /** + * 步骤 3: 更新 + * 根据 WiFi/GNSS 测量结果，更新每个粒子的权重 + * @param measurement 包含测量位置和精度的对象 + */ + public void updateWeights(Measurement measurement) { + // 1. 我们需要一个变量来记录所有粒子权重的新增总和，以便后续归一化 + double totalWeight = 0; + + // 2. 遍历每一个粒子 p + for (Particle p : particles) { + // 3. 计算这个粒子到测量点的欧几里得距离的平方 + // (x1-x2)^2 + (y1-y2)^2 + // 我们用距离的平方可以避免开方运算，效率更高。 + double distanceSquared = Math.pow(p.x - measurement.x, 2) + Math.pow(p.y - measurement.y, 2); + + // 4. 使用高斯函数计算这个粒子基于本次测量的“似然度”（likelihood） + // 这就是 p(z|x) 的核心，即“在x位置看到z测量的概率” + // 公式是: (1/sqrt(2*PI*sigma^2)) * exp(-(distance^2)/(2*sigma^2)) + // 我们忽略前面的常数部分，因为它在归一化时会被消掉。 + double sigma = measurement.accuracy; + double likelihood = Math.exp(-distanceSquared / (2 * Math.pow(sigma, 2))); + + // 5. 更新粒子的权重。新的权重 = 旧的权重 * 本次计算出的似然度 + // 如果一个粒子连续多次都离测量点很近，它的权重会持续增大。 + p.weight *= likelihood; + + // 6. 累加这个新权重到总权重中 + totalWeight += p.weight; + } + + // 7. 归一化所有粒子的权重（让它们的总和等于1） + // 这是为下一步的“重采样”做准备。 + for (Particle p : particles) { + if (totalWeight > 0) { + p.weight /= totalWeight; + } else { + // 如果所有粒子权重都变成0（极少发生，但可能），给它们一个平均权重 + p.weight = 1.0 / numParticles; + } + } + } + + /** + * 步骤 4: 重采样 + * 根据当前所有粒子的权重，生成一个全新的粒子集合。 + * 权重高的粒子有更大概率被多次选中，权重低的则可能被淘汰。 + * 这个实现使用了“低方差采样”（或称“轮盘赌采样”）算法。 + */ + public void resample() { + // 1. 创建一个新的、空的列表，用来存放重采样后的新粒子 + List newParticles = new ArrayList<>(numParticles); + + // 2. 计算当前权重最大的值，用于轮盘赌算法 + double maxWeight = 0; + for (Particle p : particles) { + if (p.weight > maxWeight) { + maxWeight = p.weight; + } + } + + // 如果所有粒子权重都为0，无法进行重采样，直接返回 + if (maxWeight == 0) { + return; + } + + // 3. 轮盘赌算法的核心 + // 想象一个周长为 maxWeight * 2.0 的轮盘 + // 我们从一个随机的位置开始，然后均匀地在轮盘上走 N 步来挑选粒子（跨过小的扇区 + double beta = 0.0; + int index = random.nextInt(numParticles); // 随机选一个起始粒子 + + for (int i = 0; i < numParticles; i++) { + beta += random.nextDouble() * 2.0 * maxWeight; + + // 寻找下一个权重足够大的粒子 + while (beta > particles.get(index).weight) { + beta -= particles.get(index).weight; + index = (index + 1) % numParticles; // 循环地移动到下一个粒子 + } + + // 找到了！这个粒子 'index' 被选中了。 + // 我们不是直接把它放进去，而是创建一个它的“克隆” + Particle selected = particles.get(index); + Particle newParticle = new Particle(selected.x, selected.y, selected.floor); + + // 把这个克隆体添加到新粒子列表中 + // 注意：新粒子的权重在构造函数里已经被默认设为1.0，为下一轮做好了准备 + newParticles.add(newParticle); + } + + // 4. 最后，用这个全新的、充满了“精英”和他们“克隆体”的列表， + // 替换掉旧的、权重不均的粒子列表。 + this.particles = newParticles; + } + +// // 其他辅助方法 +// private void normalizeWeights() { ... } +// private double calculateDistance(Particle p, Position pos) { ... } +// private double gaussian(double distance, double sigma) { ... } + + /** + * 获取最终的估计位置 + * 使用所有粒子的“加权平均值”。 + * 权重越大的粒子，对最终结果的影响越大。 + * @return 估计的 Position 对象 + */ + public Position getEstimatedPosition() { + if (particles == null || particles.isEmpty()) { + return new Position(0, 0); + } + + float expectedX = 0; + float expectedY = 0; + double totalWeight = 0; + + // 遍历所有粒子，计算加权总和 + for (Particle p : particles) { + expectedX += p.x * p.weight; // 坐标乘以它的权重 + expectedY += p.y * p.weight; + totalWeight += p.weight; // 累加总权重 + } + + // 如果总权重为0（极少发生），就返回简单平均 + if (totalWeight == 0) { + float sumX = 0, sumY = 0; + for (Particle p : particles) { + sumX += p.x; + sumY += p.y; + } + return new Position(sumX / particles.size(), sumY / particles.size()); + } + + // 最终坐标 = 加权总和 / 总权重 + return new Position((float)(expectedX / totalWeight), (float)(expectedY / totalWeight)); + } +} \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/fusion/Position.java b/app/src/main/java/com/openpositioning/PositionMe/fusion/Position.java new file mode 100644 index 00000000..51291953 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/fusion/Position.java @@ -0,0 +1,11 @@ +package com.openpositioning.PositionMe.fusion; + +public class Position { + public final float x; + public final float y; + + public Position(float x, float y) { + this.x = x; + this.y = y; + } +} \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java index aeb6386a..3fb271bb 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -87,6 +87,7 @@ public class SensorFusion implements SensorEventListener { // PDR and path private PdrProcessing pdrProcessing; private PathView pathView; + private ParticleFilter particleFilter; // Sensor registration latency setting long maxReportLatencyNs = 0; @@ -645,6 +646,19 @@ public void onLocationChanged(@NonNull Location location) { state.latitude = (float) location.getLatitude(); state.longitude = (float) location.getLongitude(); recorder.addGnssData(location); + + // --- 我们添加的粒子滤波逻辑 --- + if (particleFilter != null) { + // TODO: 我们还需要把经纬度转换成平面的 X,Y 坐标 + // 假设转换后得到了 float currentX, currentY + // float accuracy = location.getAccuracy(); + // Measurement m = new Measurement(currentX, currentY, accuracy); + // particleFilter.updateWeights(m); + // particleFilter.resample(); + // Position myPos = particleFilter.getEstimatedPosition(); + // 然后把 myPos 显示到地图上！ + } + // ------------------------------ } } diff --git a/secrets.properties b/secrets.properties index f0dc54fd..f45817cb 100644 --- a/secrets.properties +++ b/secrets.properties @@ -1,6 +1,6 @@ # # Modify the variables to set your keys # -MAPS_API_KEY= -OPENPOSITIONING_API_KEY= -OPENPOSITIONING_MASTER_KEY= +MAPS_API_KEY=AIzaSyAz8k8_QCtklumwuxyclQxZhtzSbjgQDKQ +OPENPOSITIONING_API_KEY=ewireless +OPENPOSITIONING_MASTER_KEY=tkZ4QoAApy-6CBM6fKYwYA From e43c8a067f37684624d5fa46dbc3239c8fe6fee5 Mon Sep 17 00:00:00 2001 From: Junyi Lin Date: Sat, 21 Mar 2026 10:29:07 +0000 Subject: [PATCH 2/7] - Instantiated `ParticleFilter` globally inside `SensorFusion` to act as the central positioning engine. - Connected PDR updates in `SensorEventHandler`: Extracted step deltas (`deltaX`, `deltaY`) from `pdrProcessing.updatePdr` and piped them to `particleFilter.predict()` to simulate pedestrian movement. - Connected WiFi positioning in `WifiPositionManager`: Switched to the callback-based API (`createWifiPositionRequestCallback`) to retrieve absolute global coordinates (WGS84). Implemented Flat Earth Approximation to convert WGS84 Lat/Lng to local Cartesian coordinates (Easting/Northing in meters). Piped the converted local coordinates to `particleFilter.updateWeights()` and `particleFilter.resample()` to correct PDR drift. --- .../sensors/SensorEventHandler.java | 19 ++++++++ .../PositionMe/sensors/SensorFusion.java | 12 ++++- .../sensors/WifiPositionManager.java | 45 ++++++++++++++++++- 3 files changed, 73 insertions(+), 3 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java index 639fc5c2..1896e131 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java @@ -38,6 +38,9 @@ public class SensorEventHandler { // Acceleration magnitude buffer between steps private final List accelMagnitude = new ArrayList<>(); + // --- 新增：记录上一次的 PDR 坐标，用于计算 deltaX 和 deltaY --- + private float lastPdrX = 0f; + private float lastPdrY = 0f; /** * Creates a new SensorEventHandler. @@ -170,6 +173,22 @@ public void handleSensorEvent(SensorEvent sensorEvent) { this.accelMagnitude, state.orientation[0] ); + // --- 新增：给粒子滤波器喂数据 (Predict) --- + // 1. 计算出这一步走出的增量 deltaX 和 deltaY + float deltaX = newCords[0] - lastPdrX; + float deltaY = newCords[1] - lastPdrY; + + // 2. 更新记录，供下一次使用 + lastPdrX = newCords[0]; + lastPdrY = newCords[1]; + + // 3. 拿到粒子滤波器，执行预测！ + com.openpositioning.PositionMe.fusion.ParticleFilter pf = SensorFusion.getInstance().getParticleFilter(); + if (pf != null && (deltaX != 0 || deltaY != 0)) { + // 创建任务单并让所有粒子移动 + pf.predict(new com.openpositioning.PositionMe.fusion.PDRMovement(deltaX, deltaY)); + } + // ----------------------------------------- this.accelMagnitude.clear(); diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java index 3fb271bb..f4a429bd 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -87,7 +87,7 @@ public class SensorFusion implements SensorEventListener { // PDR and path private PdrProcessing pdrProcessing; private PathView pathView; - private ParticleFilter particleFilter; + private com.openpositioning.PositionMe.fusion.ParticleFilter particleFilter; // Sensor registration latency setting long maxReportLatencyNs = 0; @@ -194,6 +194,12 @@ public void update(Object[] objList) { "WiFi RTT is not supported on this device", Toast.LENGTH_LONG).show()); } + // --- 新增：初始化粒子滤波器 --- + // 假设我们用 1000 个粒子。 + // 注意：目前还没有调用 initialize() 撒点，因为撒点需要地图边界， + // 我们等后面获取到地图信息时再调用撒点。 + this.particleFilter = new com.openpositioning.PositionMe.fusion.ParticleFilter(1000); + // ------------------------------ } //endregion @@ -661,6 +667,10 @@ public void onLocationChanged(@NonNull Location location) { // ------------------------------ } } + // --- 新增：获取粒子滤波器实例 --- + public com.openpositioning.PositionMe.fusion.ParticleFilter getParticleFilter() { + return this.particleFilter; + } //endregion } diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java index 1edfd68a..8989895d 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java @@ -51,7 +51,8 @@ public WifiPositionManager(WiFiPositioning wiFiPositioning, public void update(Object[] wifiList) { this.wifiList = Stream.of(wifiList).map(o -> (Wifi) o).collect(Collectors.toList()); recorder.addWifiFingerprint(this.wifiList); - createWifiPositioningRequest(); + // 改为调用带回调的方法： + createWifiPositionRequestCallback(); } /** @@ -85,7 +86,47 @@ private void createWifiPositionRequestCallback() { this.wiFiPositioning.request(wifiFingerPrint, new WiFiPositioning.VolleyCallback() { @Override public void onSuccess(LatLng wifiLocation, int floor) { - // Handle the success response + // --- 开始接入我们的粒子滤波器 --- + + com.openpositioning.PositionMe.fusion.ParticleFilter pf = SensorFusion.getInstance().getParticleFilter(); + + if (pf != null && wifiLocation != null) { + + // 1. 获取起始点 GPS 坐标作为本地坐标系的原点 (0,0) + float[] startLoc = SensorFusion.getInstance().getGNSSLatitude(true); + double lat0 = startLoc[0]; // 起点纬度 + double lng0 = startLoc[1]; // 起点经度 + + // 2. 将 WiFi 返回的 WGS84 经纬度转换为以米为单位的局部坐标 (X, Y) + // 使用简单的平地近似公式 (Flat Earth Approximation) / 局部切面投影 + double R = 6378137.0; // 地球赤道半径（米） + double lat = wifiLocation.latitude; + double lng = wifiLocation.longitude; + + // 计算经纬度差值并转为弧度 + double dLat = Math.toRadians(lat - lat0); + double dLng = Math.toRadians(lng - lng0); + + // 计算 X 和 Y (米) + // X = 经度差 * 赤道半径 * cos(起点纬度) + // Y = 纬度差 * 赤道半径 + float x = (float) (R * dLng * Math.cos(Math.toRadians(lat0))); + float y = (float) (R * dLat); + + // 3. 创建 Measurement 对象 + // 假设 WiFi 定位误差约为 5.0 米 (如果没有具体数值，这里预设一个合理的 sigma) + com.openpositioning.PositionMe.fusion.Measurement m = + new com.openpositioning.PositionMe.fusion.Measurement(x, y, 5.0); + + // 4. 万事俱备！更新权重并重采样！ + pf.updateWeights(m); + pf.resample(); + + // 打印出融合后的最新位置，用于我们在控制台观察它是否生效 + com.openpositioning.PositionMe.fusion.Position pos = pf.getEstimatedPosition(); + Log.d("ParticleFilter", "融合后的平滑坐标 X: " + pos.x + " Y: " + pos.y); + } + // --- 粒子滤波器接入结束 --- } @Override From af4cdc0ed97bd1780e90af98e82f79dc4e4f37d5 Mon Sep 17 00:00:00 2001 From: Junyi Lin Date: Sat, 21 Mar 2026 10:54:15 +0000 Subject: [PATCH 3/7] - Modified `SensorEventHandler` to obtain the fused position from `particleFilter.getEstimatedPosition()` after each PDR step. - Piped the fused coordinates to `pathView.drawTrajectory()` instead of the raw PDR coordinates. - The on-screen trajectory now reflects the real-time, smoothed output of the sensor fusion algorithm. - Added extensive Logcat outputs for debugging and comparing raw PDR vs. fused PF coordinates. --- .../sensors/SensorEventHandler.java | 34 +++++++++++++------ .../PositionMe/sensors/SensorFusion.java | 26 ++++++++++++++ 2 files changed, 49 insertions(+), 11 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java index 1896e131..6b4e86f5 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java @@ -168,36 +168,48 @@ public void handleSensorEvent(SensorEvent sensorEvent) { + accelMagnitude.size()); } - float[] newCords = this.pdrProcessing.updatePdr( + float[] rawPdrCords = this.pdrProcessing.updatePdr( stepTime, this.accelMagnitude, state.orientation[0] ); + this.accelMagnitude.clear(); // --- 新增：给粒子滤波器喂数据 (Predict) --- // 1. 计算出这一步走出的增量 deltaX 和 deltaY - float deltaX = newCords[0] - lastPdrX; - float deltaY = newCords[1] - lastPdrY; + float deltaX = rawPdrCords[0] - lastPdrX; + float deltaY = rawPdrCords[1] - lastPdrY; // 2. 更新记录，供下一次使用 - lastPdrX = newCords[0]; - lastPdrY = newCords[1]; + lastPdrX = rawPdrCords[0]; + lastPdrY = rawPdrCords[1]; // 3. 拿到粒子滤波器，执行预测！ com.openpositioning.PositionMe.fusion.ParticleFilter pf = SensorFusion.getInstance().getParticleFilter(); + float[] finalCordsToDraw = rawPdrCords; if (pf != null && (deltaX != 0 || deltaY != 0)) { - // 创建任务单并让所有粒子移动 + // 【第 4 步】: 执行预测，让粒子云散开 pf.predict(new com.openpositioning.PositionMe.fusion.PDRMovement(deltaX, deltaY)); - } - // ----------------------------------------- - this.accelMagnitude.clear(); + // 【第 5 步 - 核心！】: 从滤波器获取融合后的平滑坐标！ + com.openpositioning.PositionMe.fusion.Position fusedPosition = pf.getEstimatedPosition(); + + // 【第 6 步】: 把我们要画的坐标，替换成这个更智能的融合坐标 + finalCordsToDraw = new float[]{fusedPosition.x, fusedPosition.y}; + + // 【第 7 步 - 可选】: 打印日志，方便我们对比观察效果 + Log.d("ParticleFilter", "PDR原始坐标 X: " + rawPdrCords[0] + " Y: " + rawPdrCords[1]); + Log.d("ParticleFilter", "PF融合坐标 X: " + fusedPosition.x + " Y: " + fusedPosition.y); + } if (recorder.isRecording()) { - this.pathView.drawTrajectory(newCords); + // 【第 8 步 - 最终绘图！】: 把我们最终算出的平滑坐标画到屏幕上 + this.pathView.drawTrajectory(finalCordsToDraw); + state.stepCounter++; + // 数据记录仍然记录原始的 PDR 坐标 recorder.addPdrData( SystemClock.uptimeMillis() - bootTime, - newCords[0], newCords[1]); + rawPdrCords[0], rawPdrCords[1]); } break; } diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java index f4a429bd..e84a21f8 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -12,6 +12,7 @@ import android.os.Looper; import android.os.SystemClock; import android.widget.Toast; +import android.util.Log; import androidx.annotation.NonNull; import androidx.preference.PreferenceManager; @@ -497,6 +498,31 @@ public float[] getGNSSLatitude(boolean start) { public void setStartGNSSLatitude(float[] startPosition) { state.startLocation[0] = startPosition[0]; state.startLocation[1] = startPosition[1]; + // --- 核心任务：初始化并激活粒子滤波器--- + + if (this.particleFilter != null) { + + // 1. 定义一个合理的初始分布范围。 + // 因为我们的 PDR 是以米为单位的，所以这里的范围也应该是米。 + // 由于我们现在在起始点，本地坐标系的原点就是 (0, 0)。 + // 我们可以假设初始不确定性在一个 20米 x 20米 的正方形区域内。 + float initialUncertainty = 10.0f; // 半径10米 + + com.openpositioning.PositionMe.fusion.MapBounds bounds = + new com.openpositioning.PositionMe.fusion.MapBounds( + -initialUncertainty, // minX + initialUncertainty, // maxX + -initialUncertainty, // minY + initialUncertainty // maxY + ); + + // 2. 万事俱备，调用 initialize 进行撒点！ + // 这会在 (0,0) 点周围 20x20 米的范围内随机撒下 1000 个粒子。 + this.particleFilter.initialize(bounds); + + Log.d("ParticleFilter", "粒子滤波器已在起始点周围初始化！"); + } + // --- 粒子滤波器激活完成 --- } /** From 91bf10b4808480262fb09f2c36de30174ec99067 Mon Sep 17 00:00:00 2001 From: Junyi Lin Date: Sat, 21 Mar 2026 17:24:14 +0000 Subject: [PATCH 4/7] Remove secrets.properties from version control --- secrets.properties | 6 ------ 1 file changed, 6 deletions(-) delete mode 100644 secrets.properties diff --git a/secrets.properties b/secrets.properties deleted file mode 100644 index f45817cb..00000000 --- a/secrets.properties +++ /dev/null @@ -1,6 +0,0 @@ -# -# Modify the variables to set your keys -# -MAPS_API_KEY=AIzaSyAz8k8_QCtklumwuxyclQxZhtzSbjgQDKQ -OPENPOSITIONING_API_KEY=ewireless -OPENPOSITIONING_MASTER_KEY=tkZ4QoAApy-6CBM6fKYwYA From a85983a09189287edc2c977ac074095f02028dc6 Mon Sep 17 00:00:00 2001 From: Junyi Lin Date: Wed, 25 Mar 2026 11:39:44 +0000 Subject: [PATCH 5/7] - Fixed the "floor jump" bug caused by alphabetical sorting of floor keys (where 'G' was placed after 'F3'). - Implemented a custom physical sorting in `FloorplanApiClient` to ensure floor sequence: LG -> GF -> F1 -> F2 -> F3. - Refactored `setCurrentFloor` to map physical floor numbers (-1, 0, 1...) to the reordered list indices correctly. - Synchronized manual UI buttons (Up/Down) with the new index structure. - Corrected initial building overlay to default to the Ground Floor (GF). --- .gitignore | 1 + .../data/remote/FloorplanApiClient.java | 17 +++++- .../PositionMe/fusion/Particle.java | 4 +- .../fragment/TrajectoryMapFragment.java | 37 ++++++++++--- .../PositionMe/sensors/SensorFusion.java | 54 +++++++++++++++---- .../PositionMe/sensors/WiFiPositioning.java | 37 +++++++------ .../sensors/WifiPositionManager.java | 10 ++-- .../PositionMe/utils/IndoorMapManager.java | 17 ++++-- .../PositionMe/utils/PdrProcessing.java | 17 +++--- 9 files changed, 144 insertions(+), 50 deletions(-) diff --git a/.gitignore b/.gitignore index d4c3a57e..4e82ab6f 100644 --- a/.gitignore +++ b/.gitignore @@ -14,3 +14,4 @@ .cxx local.properties /.idea/ +secrets.properties \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java b/app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java index cadb6037..5e98e63a 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java +++ b/app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java @@ -372,7 +372,22 @@ private List parseMapShapes(String mapShapesJson) { while (it.hasNext()) { keys.add(it.next()); } - Collections.sort(keys); + //Collections.sort(keys); + // ！！！核心修复：手动按物理高度排序，不要用 Collections.sort(keys) ！！！ + List Keys = new ArrayList<>(); + // 定义 Nucleus 楼层的物理正确顺序 + String[] physicalOrder = {"LG", "GF", "F1", "F2", "F3"}; + for (String pKey : physicalOrder) { + if (root.has(pKey)) { + Keys.add(pKey); + } + } + // 如果还有不在列表里的（容错），再把剩下的加进去 + Iterator allKeys = root.keys(); + while (allKeys.hasNext()) { + String k = allKeys.next(); + if (!Keys.contains(k)) Keys.add(k); + } for (String key : keys) { JSONObject floorCollection = root.getJSONObject(key); diff --git a/app/src/main/java/com/openpositioning/PositionMe/fusion/Particle.java b/app/src/main/java/com/openpositioning/PositionMe/fusion/Particle.java index e37f068d..4aa668d4 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/fusion/Particle.java +++ b/app/src/main/java/com/openpositioning/PositionMe/fusion/Particle.java @@ -30,7 +30,7 @@ public void move(float deltaX, float deltaY, Random random) { // random.nextGaussian() 生成一个符合标准正态分布（均值为0，标准差为1）的随机数 // 我们把它乘以一个很小的值（比如0.1），来控制噪声的大小（即标准差为0.1米） // 这意味着大部分噪声会在-0.1米到+0.1米之间，少数会更大一些。这比均匀分布更真实。 - this.x += random.nextGaussian() * 0.1; - this.y += random.nextGaussian() * 0.1; + this.x += random.nextGaussian() * 0.05; + this.y += random.nextGaussian() * 0.05; } } diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java index 479ea51b..953d903d 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java @@ -96,6 +96,8 @@ public class TrajectoryMapFragment extends Fragment { private TextView floorLabel; private Button switchColorButton; private Polygon buildingPolygon; + private long lastAuthoritativeTime = 0; + private boolean hasAuthoritativeSource = false; public TrajectoryMapFragment() { @@ -691,19 +693,32 @@ private void evaluateAutoFloor() { if (!indoorMapManager.getIsIndoorMapSet()) return; int candidateFloor; + boolean hasAuthoritativeSource = false; // 新增一个标志，判断我们是否有“权威”信源 - // Priority 1: WiFi-based floor (only if WiFi positioning has returned data) + // 👑 最高优先级：使用 WiFi 定位返回的楼层 (如果有) if (sensorFusion.getLatLngWifiPositioning() != null) { + // WiFi API 返回的楼层通常是以 0 为地面的，最可靠 candidateFloor = sensorFusion.getWifiFloor(); + hasAuthoritativeSource = true; + android.util.Log.d("FloorDebug", "楼层来源: WiFi, 原始值: " + candidateFloor); } else { - // Fallback: barometric elevation estimate + // ⚠️ 次要优先级：使用气压计估算楼层 float elevation = sensorFusion.getElevation(); float floorHeight = indoorMapManager.getFloorHeight(); - if (floorHeight <= 0) return; + + if (floorHeight <= 0) { + // 如果没有设置楼层高度，则无法计算，直接退出 + return; + } + + // 使用气压计计算楼层，同样以 0 为地面 + // Math.round() 会四舍五入到最近的整数 candidateFloor = Math.round(elevation / floorHeight); + android.util.Log.d("FloorDebug", "楼层来源: 气压计, 海拔: " + elevation + ", 计算值: " + candidateFloor); } - // Debounce: require the same floor reading for AUTO_FLOOR_DEBOUNCE_MS + // --- 防抖动逻辑 (Debounce) 保持不变 --- + // (这部分逻辑是好的，可以防止楼层在两个值之间快速跳变) long now = SystemClock.elapsedRealtime(); if (candidateFloor != lastCandidateFloor) { lastCandidateFloor = candidateFloor; @@ -712,9 +727,17 @@ private void evaluateAutoFloor() { } if (now - lastCandidateTime >= AUTO_FLOOR_DEBOUNCE_MS) { - indoorMapManager.setCurrentFloor(candidateFloor, true); - updateFloorLabel(); - // Reset timer so we don't keep re-applying the same floor + // 只有当信号源是权威的（WiFi），或者我们长时间没有权威信号时，才更新楼层 + // 这可以防止不准的气压计覆盖掉刚定位成功的 WiFi 楼层 + if (hasAuthoritativeSource || (now - lastAuthoritativeTime > 15000)) { // 超过15秒没收到WiFi才信气压计 + indoorMapManager.setCurrentFloor(candidateFloor, true); + updateFloorLabel(); + + if (hasAuthoritativeSource) { + lastAuthoritativeTime = now; // 记录上次权威更新的时间 + } + } + // 重置计时器 lastCandidateTime = now; } } diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java index e84a21f8..87d4f6ea 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -679,18 +679,50 @@ public void onLocationChanged(@NonNull Location location) { state.longitude = (float) location.getLongitude(); recorder.addGnssData(location); - // --- 我们添加的粒子滤波逻辑 --- - if (particleFilter != null) { - // TODO: 我们还需要把经纬度转换成平面的 X,Y 坐标 - // 假设转换后得到了 float currentX, currentY - // float accuracy = location.getAccuracy(); - // Measurement m = new Measurement(currentX, currentY, accuracy); - // particleFilter.updateWeights(m); - // particleFilter.resample(); - // Position myPos = particleFilter.getEstimatedPosition(); - // 然后把 myPos 显示到地图上！ + // --- 激活粒子滤波的 GNSS (GPS) 纠偏功能！ --- + com.openpositioning.PositionMe.fusion.ParticleFilter pf = getParticleFilter(); + + if (pf != null && state.startLocation != null && state.startLocation[0] != 0.0f) { + + float realAccuracy = location.getAccuracy(); + + // ！！！核心修改：智能拒止劣质 GPS 信号 ！！！ + // 在室内，GPS 误差通常会飙升到 20米、50米甚至更高。 + // 此时的坐标毫无价值，如果强行使用，会把红线带飞。 + // 设定门槛：如果误差大于 20 米，直接忽略这次 GPS 数据！ + if (realAccuracy > 20.0f) { + android.util.Log.d("ParticleFilter", "GPS 信号太差 (" + realAccuracy + "m)，果断拒绝，不进行纠偏！"); + return; // 提前结束方法，不执行 updateWeights + } + + // 1. 获取起点 GPS 坐标，作为平面坐标系的原点 (0,0) + double lat0 = state.startLocation[0]; + double lng0 = state.startLocation[1]; + + // 2. 获取当前 GPS 坐标 + double lat = location.getLatitude(); + double lng = location.getLongitude(); + + // 3. 转换为平面坐标 (米) + double R = 6378137.0; + double dLat = Math.toRadians(lat - lat0); + double dLng = Math.toRadians(lng - lng0); + float x = (float) (R * dLng * Math.cos(Math.toRadians(lat0))); + float y = (float) (R * dLat); + + // 4. 信号既然合格（小于 20米），我们就使用它真实的误差来进行更新。 + // 为了防止极个别手机报告的误差接近 0 导致崩溃，设置个下限。 + float usedSigma = Math.max(realAccuracy, 10.0f); + + com.openpositioning.PositionMe.fusion.Measurement gnssMeasurement = + new com.openpositioning.PositionMe.fusion.Measurement(x, y, usedSigma); + + // 5. 喂给粒子滤波器 + pf.updateWeights(gnssMeasurement); + pf.resample(); + + android.util.Log.d("ParticleFilter", "GNSS 优质纠偏触发！当前误差: " + usedSigma + " 米"); } - // ------------------------------ } } // --- 新增：获取粒子滤波器实例 --- diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java index dbf809dd..3ea9bca6 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java @@ -137,6 +137,7 @@ public void request( JSONObject jsonWifiFeatures, final VolleyCallback callback) Request.Method.POST, url, jsonWifiFeatures, response -> { try { + Log.d("WifiSuccess", "✅✅✅ WiFi定位API成功！服务器返回坐标！✅✅✅"); Log.d("jsonObject",response.toString()); wifiLocation = new LatLng(response.getDouble("lat"),response.getDouble("lon")); floor = response.getInt("floor"); @@ -147,25 +148,31 @@ public void request( JSONObject jsonWifiFeatures, final VolleyCallback callback) } }, error -> { - // Validation Error - if (error.networkResponse!=null && error.networkResponse.statusCode==422){ - Log.e("WiFiPositioning", "Validation Error "+ error.getMessage()); - callback.onError( "Validation Error (422): "+ error.getMessage()); - } - // Other Errors - else{ - // When Response code is available - if (error.networkResponse!=null) { - Log.e("WiFiPositioning","Response Code: " + error.networkResponse.statusCode + ", " + error.getMessage()); - callback.onError("Response Code: " + error.networkResponse.statusCode + ", " + error.getMessage()); - } - else{ - Log.e("WiFiPositioning","Error message: " + error.getMessage()); - callback.onError("Error message: " + error.getMessage()); + if (error.networkResponse != null) { + int statusCode = error.networkResponse.statusCode; + String responseBody = "无返回体数据"; + + // 提取服务器返回的真实错误信息 + if (error.networkResponse.data != null) { + try { + responseBody = new String(error.networkResponse.data, "UTF-8"); + } catch (Exception e) { + responseBody = "解析响应体失败"; + } } + + Log.e("WifiProbe", "HTTP 状态码: " + statusCode + ", 服务器回复: " + responseBody); + callback.onError("Status: " + statusCode + " Body: " + responseBody); + + } else { + // 根本没连上服务器（比如没网、超时、DNS解析失败等） + Log.e("WifiProbe", "网络底层错误: " + error.toString()); + callback.onError("Network Error: " + error.toString()); } } ); + // ！！！新增：打印我们要发送给服务器的“WiFi指纹证据” ！！！ + Log.d("WifiProbe", "==== 即将发送的 WiFi 指纹: ==== \n" + jsonWifiFeatures.toString()); // Adds the request to the request queue requestQueue.add(jsonObjectRequest); } diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java index 8989895d..a5278af7 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java @@ -49,7 +49,8 @@ public WifiPositionManager(WiFiPositioning wiFiPositioning, */ @Override public void update(Object[] wifiList) { - this.wifiList = Stream.of(wifiList).map(o -> (Wifi) o).collect(Collectors.toList()); + android.util.Log.d("WifiProbe", "太好了！接收到了WiFi扫描数据！"); + this.wifiList = Stream.of(wifiList).map(o -> (Wifi) o).filter(w -> w.getLevel() < -40).collect(Collectors.toList()); recorder.addWifiFingerprint(this.wifiList); // 改为调用带回调的方法： createWifiPositionRequestCallback(); @@ -68,6 +69,7 @@ private void createWifiPositioningRequest() { wifiFingerPrint.put(WIFI_FINGERPRINT, wifiAccessPoints); this.wiFiPositioning.request(wifiFingerPrint); } catch (JSONException e) { + android.util.Log.e("WifiProbe", "JSON打包失败: " + e.toString()); Log.e("jsonErrors", "Error creating json object" + e.toString()); } } @@ -83,11 +85,12 @@ private void createWifiPositionRequestCallback() { } JSONObject wifiFingerPrint = new JSONObject(); wifiFingerPrint.put(WIFI_FINGERPRINT, wifiAccessPoints); + android.util.Log.d("WifiProbe", "准备发送网络请求到服务器..."); this.wiFiPositioning.request(wifiFingerPrint, new WiFiPositioning.VolleyCallback() { @Override public void onSuccess(LatLng wifiLocation, int floor) { + Log.d("WifiSuccess", "✅✅✅ 融合系统已接收WiFi坐标，准备更新粒子！✅✅✅"); // --- 开始接入我们的粒子滤波器 --- - com.openpositioning.PositionMe.fusion.ParticleFilter pf = SensorFusion.getInstance().getParticleFilter(); if (pf != null && wifiLocation != null) { @@ -116,7 +119,7 @@ public void onSuccess(LatLng wifiLocation, int floor) { // 3. 创建 Measurement 对象 // 假设 WiFi 定位误差约为 5.0 米 (如果没有具体数值，这里预设一个合理的 sigma) com.openpositioning.PositionMe.fusion.Measurement m = - new com.openpositioning.PositionMe.fusion.Measurement(x, y, 5.0); + new com.openpositioning.PositionMe.fusion.Measurement(x, y, 10.0); // 4. 万事俱备！更新权重并重采样！ pf.updateWeights(m); @@ -132,6 +135,7 @@ public void onSuccess(LatLng wifiLocation, int floor) { @Override public void onError(String message) { // Handle the error response + android.util.Log.e("WifiProbe", "服务器报错了: " + message); } }); } catch (JSONException e) { diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java index f8058603..48fc83c1 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java @@ -161,14 +161,21 @@ public int getAutoFloorBias() { public void setCurrentFloor(int newFloor, boolean autoFloor) { if (currentFloorShapes == null || currentFloorShapes.isEmpty()) return; + int targetIndex = -1; + if (autoFloor) { - newFloor += getAutoFloorBias(); + // 如果是自动切换（来自WiFi/气压计的数字 0, 1, -1） + // 现在的列表顺序很整齐：-1对应0, 0对应1, 1对应2... + // 规律就是：索引 = 物理楼层 + 1 + targetIndex = newFloor + 1; + } else { + // 如果是手动按钮传入的，它传的通常就是目标 index + targetIndex = newFloor; } - if (newFloor >= 0 && newFloor < currentFloorShapes.size() - && newFloor != this.currentFloor) { - this.currentFloor = newFloor; - drawFloorShapes(newFloor); + if (targetIndex >= 0 && targetIndex < currentFloorShapes.size()) { + this.currentFloor = targetIndex; + drawFloorShapes(targetIndex); } } diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java index 9765b044..385d0d9b 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java @@ -82,16 +82,18 @@ public class PdrProcessing { public PdrProcessing(Context context) { // Initialise settings this.settings = PreferenceManager.getDefaultSharedPreferences(context); - // Check if estimate or manual values should be used - this.useManualStep = this.settings.getBoolean("manual_step_values", false); +// // Check if estimate or manual values should be used +// this.useManualStep = this.settings.getBoolean("manual_step_values", false); + // 强制开启手动步长模式，不再使用不准的自动估计算法！ + this.useManualStep = true; if(useManualStep) { try { // Retrieve manual step length - this.stepLength = this.settings.getInt("user_step_length", 75) / 100f; + this.stepLength = this.settings.getInt("user_step_length", 20) / 100f; } catch (Exception e) { // Invalid values - reset to defaults - this.stepLength = 0.75f; - this.settings.edit().putInt("user_step_length", 75).apply(); + this.stepLength = 0.20f; + this.settings.edit().putInt("user_step_length", 20).apply(); } } else { @@ -153,7 +155,7 @@ public float[] updatePdr(long currentStepEnd, List accelMagnitudeOvertim // return current position, do not update return new float[]{this.positionX, this.positionY}; } - + // Calculate step length if(!useManualStep) { //ArrayList accelMagnitudeFiltered = filter(accelMagnitudeOvertime); @@ -161,6 +163,9 @@ public float[] updatePdr(long currentStepEnd, List accelMagnitudeOvertim this.stepLength = weibergMinMax(accelMagnitudeOvertime); // System.err.println("Step Length" + stepLength); } + // ！！！强制修正：无视所有前面的算法，步长就按我们说的算！！！ + // ！！！我们先从一个极小的值开始测试，比如 30 厘米 ！！！ + stepLength = 0.70f; // Increment aggregate variables sumStepLength += stepLength; From 542620154af4ba263864e2f51357d4dae4500eb4 Mon Sep 17 00:00:00 2001 From: Junyi Lin Date: Sat, 28 Mar 2026 13:30:04 +0000 Subject: [PATCH 6/7] 1 --- .../com/openpositioning/PositionMe/utils/GeometryUtils.java | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 app/src/main/java/com/openpositioning/PositionMe/utils/GeometryUtils.java diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/GeometryUtils.java b/app/src/main/java/com/openpositioning/PositionMe/utils/GeometryUtils.java new file mode 100644 index 00000000..84d1123f --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/GeometryUtils.java @@ -0,0 +1,4 @@ +package com.openpositioning.PositionMe.utils; + +public class GeometryUtils { +} From aa77c41d6128c1149cc93454ebf48ef537066ca6 Mon Sep 17 00:00:00 2001 From: Junyi Lin Date: Sat, 28 Mar 2026 13:35:50 +0000 Subject: [PATCH 7/7] 1 --- .../data/remote/FloorplanApiClient.java | 3 - .../PositionMe/fusion/Measurement.java | 8 +- .../PositionMe/fusion/PDRMovement.java | 5 +- .../PositionMe/fusion/Particle.java | 57 ++- .../PositionMe/fusion/ParticleFilter.java | 407 +++++++++++++----- .../fragment/CorrectionFragment.java | 30 +- .../fragment/RecordingFragment.java | 12 + .../fragment/TrajectoryMapFragment.java | 118 +++-- .../sensors/SensorEventHandler.java | 65 ++- .../PositionMe/sensors/SensorFusion.java | 158 +++++-- .../PositionMe/sensors/WiFiPositioning.java | 54 +-- .../sensors/WifiPositionManager.java | 104 +++-- .../PositionMe/utils/GeometryUtils.java | 156 +++++++ .../PositionMe/utils/IndoorMapManager.java | 18 +- .../PositionMe/utils/PdrProcessing.java | 81 +++- 15 files changed, 928 insertions(+), 348 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java b/app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java index 5e98e63a..b50496a5 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java +++ b/app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java @@ -373,16 +373,13 @@ private List parseMapShapes(String mapShapesJson) { keys.add(it.next()); } //Collections.sort(keys); - // ！！！核心修复：手动按物理高度排序，不要用 Collections.sort(keys) ！！！ List Keys = new ArrayList<>(); - // 定义 Nucleus 楼层的物理正确顺序 String[] physicalOrder = {"LG", "GF", "F1", "F2", "F3"}; for (String pKey : physicalOrder) { if (root.has(pKey)) { Keys.add(pKey); } } - // 如果还有不在列表里的（容错），再把剩下的加进去 Iterator allKeys = root.keys(); while (allKeys.hasNext()) { String k = allKeys.next(); diff --git a/app/src/main/java/com/openpositioning/PositionMe/fusion/Measurement.java b/app/src/main/java/com/openpositioning/PositionMe/fusion/Measurement.java index c318607e..88f1d73e 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/fusion/Measurement.java +++ b/app/src/main/java/com/openpositioning/PositionMe/fusion/Measurement.java @@ -1,14 +1,10 @@ package com.openpositioning.PositionMe.fusion; -/** - * 这个类封装了一次外部测量的数据。 - * 来自Wi-Fi定位或GNSS定位。 - * 它包含一个坐标点和一个表示精度的值。 - */ +/** Represents a position measurement with an accuracy estimate. */ public class Measurement { public final float x; public final float y; - public final double accuracy; // 定位的精度/标准差（比如5米） + public final double accuracy; public Measurement(float x, float y, double accuracy) { this.x = x; diff --git a/app/src/main/java/com/openpositioning/PositionMe/fusion/PDRMovement.java b/app/src/main/java/com/openpositioning/PositionMe/fusion/PDRMovement.java index 82889c81..8e3e86e5 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/fusion/PDRMovement.java +++ b/app/src/main/java/com/openpositioning/PositionMe/fusion/PDRMovement.java @@ -1,9 +1,6 @@ package com.openpositioning.PositionMe.fusion; -/** - * 这个类用来封装一次PDR（行人航位推算）的移动数据。 - * x方向和y方向的位移。 - */ +/** Represents a planar PDR displacement for a single update step. */ public class PDRMovement { public final float deltaX; public final float deltaY; diff --git a/app/src/main/java/com/openpositioning/PositionMe/fusion/Particle.java b/app/src/main/java/com/openpositioning/PositionMe/fusion/Particle.java index 4aa668d4..8fb0c0d5 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/fusion/Particle.java +++ b/app/src/main/java/com/openpositioning/PositionMe/fusion/Particle.java @@ -1,36 +1,57 @@ package com.openpositioning.PositionMe.fusion; import java.util.Random; + +/** Represents a single particle in the particle filter. */ public class Particle { - float x; // x坐标 - float y; // y坐标 - int floor; // 楼层 - double weight; // 权重 + float x; + float y; + public float oldX; + public float oldY; + int floor; + double weight; - // 构造函数等... + /** + * Creates a particle with an initial position and floor. + * + * @param x initial x coordinate in meters + * @param y initial y coordinate in meters + * @param floor initial floor index + */ public Particle(float x, float y, int floor) { this.x = x; this.y = y; + this.oldX = x; + this.oldY = y; this.floor = floor; - this.weight = 1.0; // 初始权重都一样 + this.weight = 1.0; } /** - * 根据给定的位移来移动粒子，并增加一些随机噪声。 - * @param deltaX X方向的位移 - * @param deltaY Y方向的位移 - * @param random 一个随机数生成器实例 + * Moves the particle using the default motion noise. + * + * @param deltaX movement along the x axis in meters + * @param deltaY movement along the y axis in meters + * @param random random source used to sample motion noise */ public void move(float deltaX, float deltaY, Random random) { - // 这是基础移动：直接把位移加上去 + move(deltaX, deltaY, random, 0.03f); + } + + /** + * Moves the particle and adds Gaussian motion noise. + * + * @param deltaX movement along the x axis in meters + * @param deltaY movement along the y axis in meters + * @param random random source used to sample motion noise + * @param noiseStdDev standard deviation of the motion noise in meters + */ + public void move(float deltaX, float deltaY, Random random, float noiseStdDev) { + this.oldX = this.x; + this.oldY = this.y; this.x += deltaX; this.y += deltaY; - - // 这是关键的“增加噪声”部分 - // random.nextGaussian() 生成一个符合标准正态分布（均值为0，标准差为1）的随机数 - // 我们把它乘以一个很小的值（比如0.1），来控制噪声的大小（即标准差为0.1米） - // 这意味着大部分噪声会在-0.1米到+0.1米之间，少数会更大一些。这比均匀分布更真实。 - this.x += random.nextGaussian() * 0.05; - this.y += random.nextGaussian() * 0.05; + this.x += random.nextGaussian() * noiseStdDev; + this.y += random.nextGaussian() * noiseStdDev; } } diff --git a/app/src/main/java/com/openpositioning/PositionMe/fusion/ParticleFilter.java b/app/src/main/java/com/openpositioning/PositionMe/fusion/ParticleFilter.java index 0f68d07a..118b7471 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/fusion/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/fusion/ParticleFilter.java @@ -1,183 +1,142 @@ package com.openpositioning.PositionMe.fusion; +import com.google.android.gms.maps.model.LatLng; +import com.openpositioning.PositionMe.data.remote.FloorplanApiClient; +import com.openpositioning.PositionMe.utils.GeometryUtils; + import java.util.ArrayList; import java.util.List; import java.util.Random; +/** Particle filter used to fuse PDR motion with map and measurement constraints. */ public class ParticleFilter { private List particles; - private int numParticles; - private Random random = new Random(); // 在类的成员变量里创建一个随机数生成器 + private final int numParticles; + private final Random random = new Random(); + /** + * Creates a particle filter with a fixed number of particles. + * + * @param numParticles number of particles maintained by the filter + */ public ParticleFilter(int numParticles) { this.numParticles = numParticles; this.particles = new ArrayList<>(numParticles); } /** - * 步骤 1: 初始化 - * 在地图的某个区域内随机撒点 - */ - /** - * 步骤 1: 初始化 - * 在地图的某个区域内随机撒点 - * @param mapBounds 地图的边界信息 + * Initializes particles uniformly within the provided map bounds. + * + * @param mapBounds rectangular initialization bounds in local map coordinates */ public void initialize(MapBounds mapBounds) { - // 1. 清空旧的粒子列表，确保每次初始化都是全新的开始 particles.clear(); - - // 2. 计算地图的宽度和高度 float width = mapBounds.maxX - mapBounds.minX; float height = mapBounds.maxY - mapBounds.minY; - // 3. 这是一个循环，会执行 numParticles 次，i会从0变到numParticles-1 for (int i = 0; i < numParticles; i++) { - - // 4. 生成一个0.0到1.0之间的随机小数，乘以宽度，再加上最小X值 - // 这就得到了一个在 minX 和 maxX 之间的随机X坐标 float randomX = mapBounds.minX + random.nextFloat() * width; - - // 5. 同理，生成一个在 minY 和 maxY 之间的随机Y坐标 float randomY = mapBounds.minY + random.nextFloat() * height; - - // 6. 假设开始时都在0楼 - int initialFloor = 0; - - // 7. 使用这些随机生成的坐标和楼层，创建一个新的粒子对象 - Particle newParticle = new Particle(randomX, randomY, initialFloor); - - // 8. 把这个新创建的粒子添加到我们的粒子列表中 - particles.add(newParticle); + particles.add(new Particle(randomX, randomY, 0)); } } - /** - * 步骤 2: 预测 - * 根据 PDR 的移动数据，移动所有粒子 - * @param movement 包含本次移动 deltaX 和 deltaY 的对象 + * Applies a PDR motion update to all particles. + * + * @param movement PDR motion in local map coordinates */ public void predict(PDRMovement movement) { - // 1. 获取本次移动的x和y方向的距离 - float dx = movement.deltaX; - float dy = movement.deltaY; + predict(movement, 1); + } - // 2. 遍历粒子列表中的每一个粒子 p - for (Particle p : particles) { - // 3. 调用该粒子自己的 move 方法，让它移动 - // 我们把移动距离和粒子滤波器自己的随机数生成器传给它 - p.move(dx, dy, this.random); + /** + * Applies a subdivided PDR motion update to all particles. + * + * @param movement PDR motion in local map coordinates + * @param subdivisions number of substeps used for this update + */ + public void predict(PDRMovement movement, int subdivisions) { + int safeSubdivisions = Math.max(1, subdivisions); + float dx = movement.deltaX / safeSubdivisions; + float dy = movement.deltaY / safeSubdivisions; + float noiseStdDev = 0.03f / (float) Math.sqrt(safeSubdivisions); + + for (Particle particle : particles) { + particle.move(dx, dy, this.random, noiseStdDev); } } /** - * 步骤 3: 更新 - * 根据 WiFi/GNSS 测量结果，更新每个粒子的权重 - * @param measurement 包含测量位置和精度的对象 + * Updates particle weights using an external position measurement. + * + * @param measurement measured position and uncertainty + * @param walls unused, kept to match the current call sites + * @param startLocation unused, kept to match the current call sites */ - public void updateWeights(Measurement measurement) { - // 1. 我们需要一个变量来记录所有粒子权重的新增总和，以便后续归一化 + public void updateWeights( + Measurement measurement, + List walls, + LatLng startLocation) { double totalWeight = 0; - // 2. 遍历每一个粒子 p - for (Particle p : particles) { - // 3. 计算这个粒子到测量点的欧几里得距离的平方 - // (x1-x2)^2 + (y1-y2)^2 - // 我们用距离的平方可以避免开方运算，效率更高。 - double distanceSquared = Math.pow(p.x - measurement.x, 2) + Math.pow(p.y - measurement.y, 2); - - // 4. 使用高斯函数计算这个粒子基于本次测量的“似然度”（likelihood） - // 这就是 p(z|x) 的核心，即“在x位置看到z测量的概率” - // 公式是: (1/sqrt(2*PI*sigma^2)) * exp(-(distance^2)/(2*sigma^2)) - // 我们忽略前面的常数部分，因为它在归一化时会被消掉。 + for (Particle particle : particles) { + double distanceSquared = + Math.pow(particle.x - measurement.x, 2) + + Math.pow(particle.y - measurement.y, 2); double sigma = measurement.accuracy; double likelihood = Math.exp(-distanceSquared / (2 * Math.pow(sigma, 2))); - - // 5. 更新粒子的权重。新的权重 = 旧的权重 * 本次计算出的似然度 - // 如果一个粒子连续多次都离测量点很近，它的权重会持续增大。 - p.weight *= likelihood; - - // 6. 累加这个新权重到总权重中 - totalWeight += p.weight; + particle.weight *= likelihood; + totalWeight += particle.weight; } - // 7. 归一化所有粒子的权重（让它们的总和等于1） - // 这是为下一步的“重采样”做准备。 - for (Particle p : particles) { + for (Particle particle : particles) { if (totalWeight > 0) { - p.weight /= totalWeight; + particle.weight /= totalWeight; } else { - // 如果所有粒子权重都变成0（极少发生，但可能），给它们一个平均权重 - p.weight = 1.0 / numParticles; + particle.weight = 1.0 / numParticles; } } } - /** - * 步骤 4: 重采样 - * 根据当前所有粒子的权重，生成一个全新的粒子集合。 - * 权重高的粒子有更大概率被多次选中，权重低的则可能被淘汰。 - * 这个实现使用了“低方差采样”（或称“轮盘赌采样”）算法。 - */ + /** Resamples particles using roulette-wheel sampling. */ public void resample() { - // 1. 创建一个新的、空的列表，用来存放重采样后的新粒子 List newParticles = new ArrayList<>(numParticles); - // 2. 计算当前权重最大的值，用于轮盘赌算法 double maxWeight = 0; - for (Particle p : particles) { - if (p.weight > maxWeight) { - maxWeight = p.weight; + for (Particle particle : particles) { + if (particle.weight > maxWeight) { + maxWeight = particle.weight; } } - // 如果所有粒子权重都为0，无法进行重采样，直接返回 if (maxWeight == 0) { return; } - // 3. 轮盘赌算法的核心 - // 想象一个周长为 maxWeight * 2.0 的轮盘 - // 我们从一个随机的位置开始，然后均匀地在轮盘上走 N 步来挑选粒子（跨过小的扇区 double beta = 0.0; - int index = random.nextInt(numParticles); // 随机选一个起始粒子 + int index = random.nextInt(numParticles); for (int i = 0; i < numParticles; i++) { beta += random.nextDouble() * 2.0 * maxWeight; - // 寻找下一个权重足够大的粒子 while (beta > particles.get(index).weight) { beta -= particles.get(index).weight; - index = (index + 1) % numParticles; // 循环地移动到下一个粒子 + index = (index + 1) % numParticles; } - // 找到了！这个粒子 'index' 被选中了。 - // 我们不是直接把它放进去，而是创建一个它的“克隆” Particle selected = particles.get(index); - Particle newParticle = new Particle(selected.x, selected.y, selected.floor); - - // 把这个克隆体添加到新粒子列表中 - // 注意：新粒子的权重在构造函数里已经被默认设为1.0，为下一轮做好了准备 - newParticles.add(newParticle); + newParticles.add(new Particle(selected.x, selected.y, selected.floor)); } - // 4. 最后，用这个全新的、充满了“精英”和他们“克隆体”的列表， - // 替换掉旧的、权重不均的粒子列表。 this.particles = newParticles; } -// // 其他辅助方法 -// private void normalizeWeights() { ... } -// private double calculateDistance(Particle p, Position pos) { ... } -// private double gaussian(double distance, double sigma) { ... } - /** - * 获取最终的估计位置 - * 使用所有粒子的“加权平均值”。 - * 权重越大的粒子，对最终结果的影响越大。 - * @return 估计的 Position 对象 + * Returns the weighted mean particle position. + * + * @return estimated position in local map coordinates */ public Position getEstimatedPosition() { if (particles == null || particles.isEmpty()) { @@ -188,24 +147,238 @@ public Position getEstimatedPosition() { float expectedY = 0; double totalWeight = 0; - // 遍历所有粒子，计算加权总和 - for (Particle p : particles) { - expectedX += p.x * p.weight; // 坐标乘以它的权重 - expectedY += p.y * p.weight; - totalWeight += p.weight; // 累加总权重 + for (Particle particle : particles) { + expectedX += particle.x * particle.weight; + expectedY += particle.y * particle.weight; + totalWeight += particle.weight; } - // 如果总权重为0（极少发生），就返回简单平均 if (totalWeight == 0) { - float sumX = 0, sumY = 0; - for (Particle p : particles) { - sumX += p.x; - sumY += p.y; + float sumX = 0; + float sumY = 0; + for (Particle particle : particles) { + sumX += particle.x; + sumY += particle.y; } return new Position(sumX / particles.size(), sumY / particles.size()); } - // 最终坐标 = 加权总和 / 总权重 - return new Position((float)(expectedX / totalWeight), (float)(expectedY / totalWeight)); + return new Position((float) (expectedX / totalWeight), (float) (expectedY / totalWeight)); + } + + /** + * Returns a map-aware estimated position. + * + *

If the weighted mean falls inside a blocked polygon, the filter falls back to the + * highest-weight valid particle. + * + * @param walls blocking map features + * @param startLocation origin used to convert local coordinates to latitude and longitude + * @return estimated position in local map coordinates + */ + public Position getEstimatedPosition( + List walls, LatLng startLocation) { + Position estimate = getEstimatedPosition(); + if (walls == null || startLocation == null || walls.isEmpty()) { + return estimate; + } + + LatLng estimatedLatLng = toLatLng(estimate.x, estimate.y, startLocation); + if (!isBlockedPosition(estimatedLatLng, walls)) { + return estimate; + } + + Particle bestParticle = null; + for (Particle particle : particles) { + LatLng particleLatLng = toLatLng(particle.x, particle.y, startLocation); + if (isBlockedPosition(particleLatLng, walls)) { + continue; + } + if (bestParticle == null || particle.weight > bestParticle.weight) { + bestParticle = particle; + } + } + + if (bestParticle != null) { + return new Position(bestParticle.x, bestParticle.y); + } + + return estimate; + } + + /** + * Returns the current estimated position converted to geographic coordinates. + * + * @param walls blocking map features + * @param startLocation origin used to convert local coordinates to latitude and longitude + * @return estimated latitude and longitude, or {@code null} if the start location is unavailable + */ + public LatLng getEstimatedLatLng( + List walls, LatLng startLocation) { + if (startLocation == null) { + return null; + } + Position estimate = getEstimatedPosition(walls, startLocation); + return toLatLng(estimate.x, estimate.y, startLocation); + } + + /** + * Returns a horizontal motion scale for special indoor areas. + * + * @param features current floor map features + * @param startLocation origin used to convert local coordinates to latitude and longitude + * @param elevatorDetected whether elevator motion is currently detected + * @return horizontal motion scale applied before prediction + */ + public float getHorizontalMovementScale( + List features, + LatLng startLocation, + boolean elevatorDetected) { + if (features == null || startLocation == null || features.isEmpty()) { + return 1.0f; + } + + Position estimate = getEstimatedPosition(features, startLocation); + LatLng estimatedLatLng = toLatLng(estimate.x, estimate.y, startLocation); + + if (isInsideIndoorType(estimatedLatLng, features, "elevator")) { + return elevatorDetected ? 0.05f : 0.15f; + } + if (isInsideIndoorType(estimatedLatLng, features, "stairs")) { + return 0.18f; + } + return 1.0f; + } + + /** + * Suppresses particles that cross blocking geometry or end inside blocked polygons. + * + * @param walls blocking map features + * @param startLocation origin used to convert local coordinates to latitude and longitude + */ + public void applyMapMatching( + List walls, LatLng startLocation) { + if (walls == null || startLocation == null || particles.isEmpty()) { + return; + } + + boolean allDead = true; + + for (Particle particle : particles) { + LatLng oldPos = toLatLng(particle.oldX, particle.oldY, startLocation); + LatLng newPos = toLatLng(particle.x, particle.y, startLocation); + boolean hitWall = isPathBlocked(oldPos, newPos, walls) + || isBlockedPosition(newPos, walls); + + if (hitWall) { + particle.weight = 0.0; + } else { + allDead = false; + } + } + + if (allDead) { + for (Particle particle : particles) { + particle.x = particle.oldX + (float) (random.nextGaussian() * 0.1); + particle.y = particle.oldY + (float) (random.nextGaussian() * 0.1); + particle.weight = 1.0; + } + return; + } + + double totalWeight = 0; + for (Particle particle : particles) { + totalWeight += particle.weight; + } + + if (totalWeight > 0 && totalWeight < particles.size()) { + for (Particle particle : particles) { + particle.weight /= totalWeight; + } + resample(); + android.util.Log.d("MapMatch", "Blocked particles were removed and resampled."); + } + } + + private LatLng toLatLng(float x, float y, LatLng startLocation) { + double radius = 6378137.0; + double lat0Rad = Math.toRadians(startLocation.latitude); + double dLat = y / radius; + double dLng = x / (radius * Math.cos(lat0Rad)); + return new LatLng( + startLocation.latitude + Math.toDegrees(dLat), + startLocation.longitude + Math.toDegrees(dLng)); + } + + private boolean isPathBlocked( + LatLng oldPos, LatLng newPos, List walls) { + for (FloorplanApiClient.MapShapeFeature wall : walls) { + if (!isBlockingFeature(wall)) { + continue; + } + for (List part : wall.getParts()) { + if (part == null || part.size() < 2) { + continue; + } + for (int i = 0; i < part.size() - 1; i++) { + if (GeometryUtils.doSegmentsIntersect(oldPos, newPos, part.get(i), part.get(i + 1))) { + return true; + } + } + if (part.size() > 2 + && GeometryUtils.doSegmentsIntersect( + oldPos, newPos, part.get(part.size() - 1), part.get(0))) { + return true; + } + } + } + return false; + } + + private boolean isBlockedPosition( + LatLng position, List walls) { + for (FloorplanApiClient.MapShapeFeature wall : walls) { + if (!isBlockingFeature(wall)) { + continue; + } + if (!"Polygon".equals(wall.getGeometryType()) + && !"MultiPolygon".equals(wall.getGeometryType())) { + continue; + } + for (List part : wall.getParts()) { + if (part != null && part.size() >= 3 + && GeometryUtils.isPointInPolygon(position, part)) { + return true; + } + } + } + return false; + } + + private boolean isInsideIndoorType( + LatLng position, + List features, + String indoorType) { + for (FloorplanApiClient.MapShapeFeature feature : features) { + if (!indoorType.equals(feature.getIndoorType())) { + continue; + } + if (!"Polygon".equals(feature.getGeometryType()) + && !"MultiPolygon".equals(feature.getGeometryType())) { + continue; + } + for (List part : feature.getParts()) { + if (part != null && part.size() >= 3 + && GeometryUtils.isPointInPolygon(position, part)) { + return true; + } + } + } + return false; + } + + private boolean isBlockingFeature(FloorplanApiClient.MapShapeFeature feature) { + return "wall".equals(feature.getIndoorType()) + || "unaccessible".equals(feature.getIndoorType()); } -} \ No newline at end of file +} diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/CorrectionFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/CorrectionFragment.java index 165b9e16..f2dcbe80 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/CorrectionFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/CorrectionFragment.java @@ -31,16 +31,17 @@ import com.google.android.gms.maps.model.MarkerOptions; /** - * A simple {@link Fragment} subclass. Corrections Fragment is displayed after a recording session - * is finished to enable manual adjustments to the PDR. The adjustments are not saved as of now. + * Lets the user review and manually rescale the recorded PDR trajectory after a session. + * + *

Corrections made in this screen are applied to the preview only and are not persisted. */ public class CorrectionFragment extends Fragment { - //Map variable + /** Google Map used to preview the corrected trajectory. */ public GoogleMap mMap; - //Button to go to next + /** Advances to the next screen after reviewing the correction. */ private Button button; - //Singleton SensorFusion class + /** Shared sensor fusion pipeline instance. */ private SensorFusion sensorFusion = SensorFusion.getInstance(); private TextView averageStepLengthText; private EditText stepLengthInput; @@ -53,7 +54,7 @@ public class CorrectionFragment extends Fragment { private PathView pathView; public CorrectionFragment() { - // Required empty public constructor + // Required empty public constructor. } @Override @@ -65,13 +66,13 @@ public View onCreateView(LayoutInflater inflater, ViewGroup container, } View rootView = inflater.inflate(R.layout.fragment_correction, container, false); - // Validate trajectory quality before uploading + // Validate trajectory quality before uploading. validateAndUpload(); - //Obtain start position + // Obtain the start position. float[] startPosition = sensorFusion.getGNSSLatitude(true); - // Initialize map fragment + // Initialize the embedded map fragment. SupportMapFragment supportMapFragment=(SupportMapFragment) getChildFragmentManager().findFragmentById(R.id.map); @@ -85,11 +86,11 @@ public void onMapReady(GoogleMap map) { mMap.getUiSettings().setRotateGesturesEnabled(true); mMap.getUiSettings().setScrollGesturesEnabled(true); - // Add a marker at the start position + // Add a marker at the start position. start = new LatLng(startPosition[0], startPosition[1]); mMap.addMarker(new MarkerOptions().position(start).title("Start Position")); - // Calculate zoom for demonstration + // Calculate a zoom level that matches the rendered path scale. double zoom = Math.log(156543.03392f * Math.cos(startPosition[0] * Math.PI / 180) * scalingRatio) / Math.log(2); mMap.moveCamera(CameraUpdateFactory.newLatLngZoom(start, (float) zoom)); @@ -111,11 +112,11 @@ public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceStat averageStepLengthText.setText(getString(R.string.averageStepLgn) + ": " + String.format("%.2f", averageStepLength)); - // Listen for ENTER key + // Listen for the Enter key to apply a new step length. this.stepLengthInput.setOnKeyListener((v, keyCode, event) -> { if (keyCode == KeyEvent.KEYCODE_ENTER) { newStepLength = Float.parseFloat(changedText.toString()); - // Rescale path + // Rescale the previewed path. sensorFusion.redrawPath(newStepLength / averageStepLength); averageStepLengthText.setText(getString(R.string.averageStepLgn) + ": " + String.format("%.2f", newStepLength)); @@ -170,7 +171,6 @@ private void validateAndUpload() { TrajectoryValidator.ValidationResult result = sensorFusion.validateTrajectory(); if (result.isClean()) { - // All checks passed — upload immediately Log.i("CorrectionFragment", "Trajectory validation passed, uploading"); sensorFusion.sendTrajectoryToCloud(); return; @@ -180,7 +180,6 @@ private void validateAndUpload() { Log.w("CorrectionFragment", "Trajectory quality issues:\n" + summary); if (!result.isPassed()) { - // Blocking errors exist — warn strongly but still allow upload new AlertDialog.Builder(requireContext()) .setTitle(R.string.validation_error_title) .setMessage(getString(R.string.validation_error_message, summary)) @@ -193,7 +192,6 @@ private void validateAndUpload() { .setCancelable(false) .show(); } else { - // Only warnings — show lighter dialog new AlertDialog.Builder(requireContext()) .setTitle(R.string.validation_warning_title) .setMessage(getString(R.string.validation_warning_message, summary)) diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java index 340843ca..955a087f 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java @@ -302,6 +302,18 @@ private void updateUIandPosition() { } } + if (trajectoryMapFragment != null) { + LatLng newWifi = sensorFusion.popLatestWifiLocation(); + if (newWifi != null) { + trajectoryMapFragment.drawWifiUpdatePoint(newWifi); + } + + LatLng newGnss = sensorFusion.popLatestGnssLocation(); + if (newGnss != null) { + trajectoryMapFragment.drawGnssUpdatePoint(newGnss); + } + } + // =================================== // Update previous previousPosX = pdrValues[0]; previousPosY = pdrValues[1]; diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java index 953d903d..ff6771bc 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java @@ -81,6 +81,8 @@ public class TrajectoryMapFragment extends Fragment { private static final String TAG = "TrajectoryMapFragment"; private static final long AUTO_FLOOR_DEBOUNCE_MS = 3000; private static final long AUTO_FLOOR_CHECK_INTERVAL_MS = 1000; + private static final int MAX_AUTO_FLOOR_STEP_WITHOUT_ELEVATOR = 1; + private static final int MAX_WIFI_BARO_FLOOR_DISAGREEMENT = 1; private Handler autoFloorHandler; private Runnable autoFloorTask; private int lastCandidateFloor = Integer.MIN_VALUE; @@ -307,7 +309,6 @@ public void onNothingSelected(AdapterView parent) {} * and append to polyline if the user actually moved. * * @param newLocation The new location to plot. - * @param orientation The user’s heading (e.g. from sensor fusion). */ public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { if (gMap == null) return; @@ -654,15 +655,8 @@ private void applyImmediateFloor() { if (sensorFusion == null || indoorMapManager == null) return; if (!indoorMapManager.getIsIndoorMapSet()) return; - int candidateFloor; - if (sensorFusion.getLatLngWifiPositioning() != null) { - candidateFloor = sensorFusion.getWifiFloor(); - } else { - float elevation = sensorFusion.getElevation(); - float floorHeight = indoorMapManager.getFloorHeight(); - if (floorHeight <= 0) return; - candidateFloor = Math.round(elevation / floorHeight); - } + Integer candidateFloor = resolveAutoFloorCandidate(); + if (candidateFloor == null) return; indoorMapManager.setCurrentFloor(candidateFloor, true); updateFloorLabel(); @@ -692,33 +686,10 @@ private void evaluateAutoFloor() { if (sensorFusion == null || indoorMapManager == null) return; if (!indoorMapManager.getIsIndoorMapSet()) return; - int candidateFloor; - boolean hasAuthoritativeSource = false; // 新增一个标志，判断我们是否有“权威”信源 - - // 👑 最高优先级：使用 WiFi 定位返回的楼层 (如果有) - if (sensorFusion.getLatLngWifiPositioning() != null) { - // WiFi API 返回的楼层通常是以 0 为地面的，最可靠 - candidateFloor = sensorFusion.getWifiFloor(); - hasAuthoritativeSource = true; - android.util.Log.d("FloorDebug", "楼层来源: WiFi, 原始值: " + candidateFloor); - } else { - // ⚠️ 次要优先级：使用气压计估算楼层 - float elevation = sensorFusion.getElevation(); - float floorHeight = indoorMapManager.getFloorHeight(); - - if (floorHeight <= 0) { - // 如果没有设置楼层高度，则无法计算，直接退出 - return; - } - - // 使用气压计计算楼层，同样以 0 为地面 - // Math.round() 会四舍五入到最近的整数 - candidateFloor = Math.round(elevation / floorHeight); - android.util.Log.d("FloorDebug", "楼层来源: 气压计, 海拔: " + elevation + ", 计算值: " + candidateFloor); - } + Integer candidateFloor = resolveAutoFloorCandidate(); + if (candidateFloor == null) return; + boolean hasAuthoritativeSource = sensorFusion.getLatLngWifiPositioning() != null; - // --- 防抖动逻辑 (Debounce) 保持不变 --- - // (这部分逻辑是好的，可以防止楼层在两个值之间快速跳变) long now = SystemClock.elapsedRealtime(); if (candidateFloor != lastCandidateFloor) { lastCandidateFloor = candidateFloor; @@ -727,20 +698,85 @@ private void evaluateAutoFloor() { } if (now - lastCandidateTime >= AUTO_FLOOR_DEBOUNCE_MS) { - // 只有当信号源是权威的（WiFi），或者我们长时间没有权威信号时，才更新楼层 - // 这可以防止不准的气压计覆盖掉刚定位成功的 WiFi 楼层 - if (hasAuthoritativeSource || (now - lastAuthoritativeTime > 15000)) { // 超过15秒没收到WiFi才信气压计 + if (hasAuthoritativeSource || (now - lastAuthoritativeTime > 15000)) { indoorMapManager.setCurrentFloor(candidateFloor, true); updateFloorLabel(); if (hasAuthoritativeSource) { - lastAuthoritativeTime = now; // 记录上次权威更新的时间 + lastAuthoritativeTime = now; } } - // 重置计时器 lastCandidateTime = now; } } + private Integer resolveAutoFloorCandidate() { + boolean hasWifiFloor = sensorFusion.getLatLngWifiPositioning() != null; + Integer wifiFloor = hasWifiFloor ? sensorFusion.getWifiFloor() : null; + Integer barometricFloor = getBarometricFloorEstimate(); + boolean elevatorDetected = sensorFusion.getElevator(); + + if (hasWifiFloor) { + if (barometricFloor != null + && Math.abs(wifiFloor - barometricFloor) > MAX_WIFI_BARO_FLOOR_DISAGREEMENT + && !elevatorDetected) { + Log.d(TAG, "Skipping WiFi auto-floor candidate due to large barometric disagreement: wifi=" + + wifiFloor + ", barometric=" + barometricFloor); + } else if (isPlausibleAutoFloorCandidate(wifiFloor, elevatorDetected)) { + Log.d(TAG, "Auto-floor candidate from WiFi: " + wifiFloor); + return wifiFloor; + } else { + Log.d(TAG, "Skipping implausible WiFi auto-floor candidate: " + wifiFloor); + } + } + + if (barometricFloor != null && isPlausibleAutoFloorCandidate(barometricFloor, elevatorDetected)) { + Log.d(TAG, "Auto-floor candidate from barometer: " + barometricFloor); + return barometricFloor; + } + + if (barometricFloor != null) { + Log.d(TAG, "Skipping implausible barometric auto-floor candidate: " + barometricFloor); + } + return null; + } + + private Integer getBarometricFloorEstimate() { + float floorHeight = indoorMapManager.getFloorHeight(); + if (floorHeight <= 0) return null; + return Math.round(sensorFusion.getElevation() / floorHeight); + } + + private boolean isPlausibleAutoFloorCandidate(int candidateFloor, boolean elevatorDetected) { + int currentLogicalFloor = indoorMapManager.getCurrentFloor() - indoorMapManager.getAutoFloorBias(); + if (candidateFloor == currentLogicalFloor) return true; + if (elevatorDetected) return Math.abs(candidateFloor - currentLogicalFloor) <= 2; + return Math.abs(candidateFloor - currentLogicalFloor) <= MAX_AUTO_FLOOR_STEP_WITHOUT_ELEVATOR; + } + + /** + */ + public void drawGnssUpdatePoint(LatLng location) { + if (gMap == null || location == null) return; + gMap.addCircle(new CircleOptions() + .center(location) + .radius(0.75) + .fillColor(Color.BLUE) + .strokeWidth(0) + .zIndex(2)); + } + + /** + */ + public void drawWifiUpdatePoint(LatLng location) { + if (gMap == null || location == null) return; + gMap.addCircle(new CircleOptions() + .center(location) + .radius(0.75) + .fillColor(Color.GREEN) + .strokeWidth(0) + .zIndex(3)); + } + //endregion } diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java index 6b4e86f5..15d55775 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java @@ -6,12 +6,14 @@ import android.os.SystemClock; import android.util.Log; +import com.openpositioning.PositionMe.data.remote.FloorplanApiClient; import com.openpositioning.PositionMe.utils.PathView; import com.openpositioning.PositionMe.utils.PdrProcessing; import java.util.ArrayList; import java.util.HashMap; import java.util.List; +import com.google.android.gms.maps.model.LatLng; /** * Handles sensor event dispatching for all registered movement sensors. @@ -24,7 +26,10 @@ public class SensorEventHandler { private static final float ALPHA = 0.8f; private static final long LARGE_GAP_THRESHOLD_MS = 500; - + private static final float MAP_MATCH_SUBSTEP_METERS = 0.75f; + private static final int MAX_MAP_MATCH_SUBDIVISIONS = 2; + private static final long TURN_CONFIRM_WINDOW_MS = 850; + private static final float TURN_CONFIRM_GYRO_THRESHOLD_RAD_S = 0.6f; private final SensorState state; private final PdrProcessing pdrProcessing; private final PathView pathView; @@ -34,11 +39,12 @@ public class SensorEventHandler { private final HashMap lastEventTimestamps = new HashMap<>(); private final HashMap eventCounts = new HashMap<>(); private long lastStepTime = 0; + private long lastHeadingMotionTimestamp = 0; private long bootTime; // Acceleration magnitude buffer between steps private final List accelMagnitude = new ArrayList<>(); - // --- 新增：记录上一次的 PDR 坐标，用于计算 deltaX 和 deltaY --- + // Track the previous raw PDR position to compute per-step deltas. private float lastPdrX = 0f; private float lastPdrY = 0f; @@ -101,7 +107,9 @@ public void handleSensorEvent(SensorEvent sensorEvent) { state.angularVelocity[0] = sensorEvent.values[0]; state.angularVelocity[1] = sensorEvent.values[1]; state.angularVelocity[2] = sensorEvent.values[2]; - + if (Math.abs(sensorEvent.values[2]) >= TURN_CONFIRM_GYRO_THRESHOLD_RAD_S) { + lastHeadingMotionTimestamp = currentTime; + } case Sensor.TYPE_LINEAR_ACCELERATION: state.filteredAcc[0] = sensorEvent.values[0]; state.filteredAcc[1] = sensorEvent.values[1]; @@ -167,46 +175,60 @@ public void handleSensorEvent(SensorEvent sensorEvent) { "stepDetection triggered, accelMagnitude size = " + accelMagnitude.size()); } - float[] rawPdrCords = this.pdrProcessing.updatePdr( stepTime, this.accelMagnitude, - state.orientation[0] + state.orientation[0], + state.angularVelocity[2], + currentTime - lastHeadingMotionTimestamp <= TURN_CONFIRM_WINDOW_MS ); this.accelMagnitude.clear(); - // --- 新增：给粒子滤波器喂数据 (Predict) --- - // 1. 计算出这一步走出的增量 deltaX 和 deltaY + // Derive the raw PDR delta for this detected step. float deltaX = rawPdrCords[0] - lastPdrX; float deltaY = rawPdrCords[1] - lastPdrY; - // 2. 更新记录，供下一次使用 + // Persist the raw PDR position for the next step. lastPdrX = rawPdrCords[0]; lastPdrY = rawPdrCords[1]; - // 3. 拿到粒子滤波器，执行预测！ + // Run prediction and map matching before drawing the fused position. com.openpositioning.PositionMe.fusion.ParticleFilter pf = SensorFusion.getInstance().getParticleFilter(); float[] finalCordsToDraw = rawPdrCords; if (pf != null && (deltaX != 0 || deltaY != 0)) { - // 【第 4 步】: 执行预测，让粒子云散开 - pf.predict(new com.openpositioning.PositionMe.fusion.PDRMovement(deltaX, deltaY)); - - // 【第 5 步 - 核心！】: 从滤波器获取融合后的平滑坐标！ - com.openpositioning.PositionMe.fusion.Position fusedPosition = pf.getEstimatedPosition(); - - // 【第 6 步】: 把我们要画的坐标，替换成这个更智能的融合坐标 + // Split larger moves into a small number of map-matching substeps. + int subdivisions = Math.max(1, Math.min(MAX_MAP_MATCH_SUBDIVISIONS, + (int) Math.ceil(Math.hypot(deltaX, deltaY) / MAP_MATCH_SUBSTEP_METERS))); + + // Apply area-based horizontal motion scaling and wall constraints. + List walls = SensorFusion.getInstance().getCurrentWalls(); + LatLng startLocation = new LatLng(state.startLocation[0], state.startLocation[1]); + float horizontalMovementScale = pf.getHorizontalMovementScale( + walls, startLocation, state.elevator); + float adjustedDeltaX = deltaX * horizontalMovementScale; + float adjustedDeltaY = deltaY * horizontalMovementScale; + for (int i = 0; i < subdivisions; i++) { + pf.predict(new com.openpositioning.PositionMe.fusion.PDRMovement(adjustedDeltaX, adjustedDeltaY), subdivisions); + pf.applyMapMatching(walls, startLocation); + } + + // Draw the fused position instead of the raw PDR position. + com.openpositioning.PositionMe.fusion.Position fusedPosition = + pf.getEstimatedPosition(walls, startLocation); + + // Debug logging for comparing raw PDR and particle-filter output. finalCordsToDraw = new float[]{fusedPosition.x, fusedPosition.y}; - // 【第 7 步 - 可选】: 打印日志，方便我们对比观察效果 - Log.d("ParticleFilter", "PDR原始坐标 X: " + rawPdrCords[0] + " Y: " + rawPdrCords[1]); - Log.d("ParticleFilter", "PF融合坐标 X: " + fusedPosition.x + " Y: " + fusedPosition.y); + // Debug logging for comparing raw PDR and particle-filter output. + Log.d("ParticleFilter", "X: " + rawPdrCords[0] + " Y: " + rawPdrCords[1]); + Log.d("ParticleFilter", "X: " + fusedPosition.x + " Y: " + fusedPosition.y); } if (recorder.isRecording()) { - // 【第 8 步 - 最终绘图！】: 把我们最终算出的平滑坐标画到屏幕上 + // Draw the fused trajectory on screen. this.pathView.drawTrajectory(finalCordsToDraw); state.stepCounter++; - // 数据记录仍然记录原始的 PDR 坐标 + // Keep the recorded PDR trace in the raw local coordinate frame. recorder.addPdrData( SystemClock.uptimeMillis() - bootTime, rawPdrCords[0], rawPdrCords[1]); @@ -235,4 +257,5 @@ public void logSensorFrequencies() { void resetBootTime(long newBootTime) { this.bootTime = newBootTime; } + } diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java index 87d4f6ea..6a5a7339 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -53,6 +53,9 @@ public class SensorFusion implements SensorEventListener { //region Static variables private static final SensorFusion sensorFusion = new SensorFusion(); //endregion + private static final double START_WIFI_WEIGHT = 0.7; + private static final double START_GNSS_WEIGHT = 0.3; + private static final double MAX_START_BLEND_DISTANCE_METERS = 25.0; //region Instance variables private Context appContext; @@ -96,6 +99,9 @@ public class SensorFusion implements SensorEventListener { // Floorplan API cache (latest result from start-location step) private final Map floorplanBuildingCache = new HashMap<>(); + private List currentWalls = null; + private LatLng latestWifiLocation = null; + private LatLng latestGnssLocation = null; //endregion //region Initialisation @@ -123,7 +129,6 @@ public static SensorFusion getInstance() { * the system for data collection.

* * @param context application context for permissions and device access. - * * @see MovementSensor handling all SensorManager based data collection devices. * @see ServerCommunications handling communication with the server. * @see GNSSDataProcessor for location data processing. @@ -195,10 +200,10 @@ public void update(Object[] objList) { "WiFi RTT is not supported on this device", Toast.LENGTH_LONG).show()); } - // --- 新增：初始化粒子滤波器 --- - // 假设我们用 1000 个粒子。 - // 注意：目前还没有调用 initialize() 撒点，因为撒点需要地图边界， - // 我们等后面获取到地图信息时再调用撒点。 + // Initialise the particle filter once. Particle placement still happens later + // after map bounds become available. + this.particleFilter = new com.openpositioning.PositionMe.fusion.ParticleFilter(1000); + // ------------------------------ this.particleFilter = new com.openpositioning.PositionMe.fusion.ParticleFilter(1000); // ------------------------------ } @@ -217,9 +222,12 @@ public void onSensorChanged(SensorEvent sensorEvent) { eventHandler.handleSensorEvent(sensorEvent); } - /** {@inheritDoc} */ + /** + * {@inheritDoc} + */ @Override - public void onAccuracyChanged(Sensor sensor, int i) {} + public void onAccuracyChanged(Sensor sensor, int i) { + } //endregion @@ -496,17 +504,14 @@ public float[] getGNSSLatitude(boolean start) { * @param startPosition contains the initial location set by the user */ public void setStartGNSSLatitude(float[] startPosition) { - state.startLocation[0] = startPosition[0]; - state.startLocation[1] = startPosition[1]; - // --- 核心任务：初始化并激活粒子滤波器--- + LatLng blendedStart = resolveBlendedStartLocation(startPosition); + state.startLocation[0] = (float) blendedStart.latitude; + state.startLocation[1] = (float) blendedStart.longitude; if (this.particleFilter != null) { - - // 1. 定义一个合理的初始分布范围。 - // 因为我们的 PDR 是以米为单位的，所以这里的范围也应该是米。 - // 由于我们现在在起始点，本地坐标系的原点就是 (0, 0)。 - // 我们可以假设初始不确定性在一个 20米 x 20米 的正方形区域内。 - float initialUncertainty = 10.0f; // 半径10米 + // The local coordinate origin is the user-selected start point. + // Initialise particles within a symmetric uncertainty box around that point. + float initialUncertainty = 10.0f; // 10-meter half-width com.openpositioning.PositionMe.fusion.MapBounds bounds = new com.openpositioning.PositionMe.fusion.MapBounds( @@ -516,13 +521,47 @@ public void setStartGNSSLatitude(float[] startPosition) { initialUncertainty // maxY ); - // 2. 万事俱备，调用 initialize 进行撒点！ - // 这会在 (0,0) 点周围 20x20 米的范围内随机撒下 1000 个粒子。 + // Initialise particles around the selected start point. this.particleFilter.initialize(bounds); - Log.d("ParticleFilter", "粒子滤波器已在起始点周围初始化！"); + Log.d("ParticleFilter", "Particle filter initialised around the selected start point."); + } + // Particle filter start-up is complete. + } + + private LatLng resolveBlendedStartLocation(float[] startPosition) { + LatLng gnssStart = new LatLng(startPosition[0], startPosition[1]); + LatLng wifiStart = getLatLngWifiPositioning(); + + if (wifiStart == null) { + Log.d("StartLocation", "Using GNSS-only start location"); + return gnssStart; + } + + double distanceMeters = distanceMeters(gnssStart, wifiStart); + if (distanceMeters > MAX_START_BLEND_DISTANCE_METERS) { + Log.d("StartLocation", "Skipping WiFi/GNSS start blending, distance too large: " + + distanceMeters); + return gnssStart; } - // --- 粒子滤波器激活完成 --- + + double blendedLat = wifiStart.latitude * START_WIFI_WEIGHT + gnssStart.latitude * START_GNSS_WEIGHT; + double blendedLng = wifiStart.longitude * START_WIFI_WEIGHT + gnssStart.longitude * START_GNSS_WEIGHT; + Log.d("StartLocation", "Using blended start location, wifiWeight=" + + START_WIFI_WEIGHT + ", gnssWeight=" + START_GNSS_WEIGHT + + ", distance=" + distanceMeters); + return new LatLng(blendedLat, blendedLng); + } + + private double distanceMeters(LatLng a, LatLng b) { + double radius = 6378137.0; + double lat1 = Math.toRadians(a.latitude); + double lat2 = Math.toRadians(b.latitude); + double dLat = lat2 - lat1; + double dLng = Math.toRadians(b.longitude - a.longitude); + double x = dLng * Math.cos((lat1 + lat2) / 2.0); + double y = dLat; + return Math.sqrt(x * x + y * y) * radius; } /** @@ -679,56 +718,101 @@ public void onLocationChanged(@NonNull Location location) { state.longitude = (float) location.getLongitude(); recorder.addGnssData(location); - // --- 激活粒子滤波的 GNSS (GPS) 纠偏功能！ --- + // Apply GNSS corrections only when the reported accuracy is acceptable. com.openpositioning.PositionMe.fusion.ParticleFilter pf = getParticleFilter(); if (pf != null && state.startLocation != null && state.startLocation[0] != 0.0f) { float realAccuracy = location.getAccuracy(); - // ！！！核心修改：智能拒止劣质 GPS 信号 ！！！ - // 在室内，GPS 误差通常会飙升到 20米、50米甚至更高。 - // 此时的坐标毫无价值，如果强行使用，会把红线带飞。 - // 设定门槛：如果误差大于 20 米，直接忽略这次 GPS 数据！ + // Ignore poor indoor GNSS fixes to avoid dragging the trajectory away. + // Indoor GPS errors often become large enough to be harmful instead of helpful. + // When that happens, skip this update entirely. + // The threshold is currently set to 20 meters. if (realAccuracy > 20.0f) { - android.util.Log.d("ParticleFilter", "GPS 信号太差 (" + realAccuracy + "m)，果断拒绝，不进行纠偏！"); - return; // 提前结束方法，不执行 updateWeights + android.util.Log.d("ParticleFilter", "GNSS fix rejected due to poor accuracy: " + realAccuracy + "m"); + return; // Skip this GNSS correction. } - // 1. 获取起点 GPS 坐标，作为平面坐标系的原点 (0,0) + // Convert the current GNSS fix into the local map coordinate system. + // Convert the current GNSS fix into the local map coordinate system. double lat0 = state.startLocation[0]; double lng0 = state.startLocation[1]; - // 2. 获取当前 GPS 坐标 double lat = location.getLatitude(); double lng = location.getLongitude(); - // 3. 转换为平面坐标 (米) double R = 6378137.0; double dLat = Math.toRadians(lat - lat0); double dLng = Math.toRadians(lng - lng0); float x = (float) (R * dLng * Math.cos(Math.toRadians(lat0))); float y = (float) (R * dLat); - // 4. 信号既然合格（小于 20米），我们就使用它真实的误差来进行更新。 - // 为了防止极个别手机报告的误差接近 0 导致崩溃，设置个下限。 + // Use the reported accuracy, but keep a lower bound for numerical stability. + // This avoids overly confident updates from unrealistic near-zero values. float usedSigma = Math.max(realAccuracy, 10.0f); com.openpositioning.PositionMe.fusion.Measurement gnssMeasurement = new com.openpositioning.PositionMe.fusion.Measurement(x, y, usedSigma); - // 5. 喂给粒子滤波器 - pf.updateWeights(gnssMeasurement); + // Feed the accepted GNSS measurement into the particle filter. + // Reuse the current map state so blocked particles can be down-weighted. + LatLng startLocation = new LatLng(state.startLocation[0], state.startLocation[1]); + LatLng gnssLoc = new LatLng(location.getLatitude(), location.getLongitude()); + setLatestGnssLocation(gnssLoc); + pf.updateWeights(gnssMeasurement, currentWalls, startLocation); pf.resample(); - android.util.Log.d("ParticleFilter", "GNSS 优质纠偏触发！当前误差: " + usedSigma + " 米"); + android.util.Log.d("ParticleFilter", "GNSS correction applied with sigma=" + usedSigma + "m"); + + android.util.Log.d("ParticleFilter", "GNSS correction applied with sigma=" + usedSigma + "m"); } } } - // --- 新增：获取粒子滤波器实例 --- + + // Returns the shared particle filter instance. + // Returns the shared particle filter instance. public com.openpositioning.PositionMe.fusion.ParticleFilter getParticleFilter() { return this.particleFilter; } + // Accessors for the current map state used by map matching and drawing. + public void setCurrentWalls(List walls) { + this.currentWalls = walls; + } + + public List getCurrentWalls() { + return this.currentWalls; + } + + public LatLng getStartLocationLatLng() { + if (state.startLocation == null || state.startLocation.length < 2) { + return null; + } + if (state.startLocation[0] == 0.0f && state.startLocation[1] == 0.0f) { + return null; + } + return new LatLng(state.startLocation[0], state.startLocation[1]); + } + + public void setLatestWifiLocation(LatLng loc) { + this.latestWifiLocation = loc; + } + + public LatLng popLatestWifiLocation() { + LatLng loc = this.latestWifiLocation; + this.latestWifiLocation = null; // Clear after consumption so the marker is drawn once. + return loc; + } + + public void setLatestGnssLocation(LatLng loc) { + this.latestGnssLocation = loc; + } + + public LatLng popLatestGnssLocation() { + LatLng loc = this.latestGnssLocation; + this.latestGnssLocation = null; // Clear after consumption so the marker is drawn once. + return loc; + } //endregion -} +} \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java index 3ea9bca6..e7359e6a 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java @@ -118,62 +118,62 @@ public void request(JSONObject jsonWifiFeatures) { /** - * Creates a POST request using the WiFi fingerprint to obtain user's location - * The POST request is issued to https://openpositioning.org/api/position/fine - * (the openpositioning API) with the WiFI fingerprint passed as the parameter. + * Creates a POST request using the WiFi fingerprint to obtain the user's location. * - * The response of the post request returns the coordinates of the WiFi position - * along with the floor of the building the user is at though a callback. + *

The POST request is issued to {@code https://openpositioning.org/api/position/fine} + * (the openpositioning API) with the WiFi fingerprint passed as the parameter. * - * A try and catch block along with error Logs have been added to keep a record of error's - * obtained while handling POST requests (for better maintainability and secure programming) + *

The response of the POST request returns the coordinates of the WiFi position + * along with the floor of the building the user is at through a callback. * - * @param jsonWifiFeatures WiFi Fingerprint from device - * @param callback callback function to allow user to use location when ready + *

A try-catch block along with error logs have been added to keep a record of errors + * obtained while handling POST requests for better maintainability and secure programming. + * + * @param jsonWifiFeatures WiFi fingerprint from the device + * @param callback callback function to handle the location response or errors */ - public void request( JSONObject jsonWifiFeatures, final VolleyCallback callback) { - // Creating the POST request using WiFi fingerprint (a JSON object) + public void request(JSONObject jsonWifiFeatures, final VolleyCallback callback) { + // Create the POST request using the WiFi fingerprint (a JSON object). JsonObjectRequest jsonObjectRequest = new JsonObjectRequest( Request.Method.POST, url, jsonWifiFeatures, response -> { try { - Log.d("WifiSuccess", "✅✅✅ WiFi定位API成功！服务器返回坐标！✅✅✅"); - Log.d("jsonObject",response.toString()); - wifiLocation = new LatLng(response.getDouble("lat"),response.getDouble("lon")); + Log.d("WifiSuccess", "Success! Server returned coordinates."); + Log.d("jsonObject", response.toString()); + + wifiLocation = new LatLng(response.getDouble("lat"), response.getDouble("lon")); floor = response.getInt("floor"); - callback.onSuccess(wifiLocation,floor); + callback.onSuccess(wifiLocation, floor); } catch (JSONException e) { - Log.e("jsonErrors","Error parsing response: "+e.getMessage()+" "+ response); + Log.e("jsonErrors", "Error parsing response: " + e.getMessage() + " " + response); callback.onError("Error parsing response: " + e.getMessage()); } }, error -> { if (error.networkResponse != null) { int statusCode = error.networkResponse.statusCode; - String responseBody = "无返回体数据"; + String responseBody = ""; - // 提取服务器返回的真实错误信息 if (error.networkResponse.data != null) { try { responseBody = new String(error.networkResponse.data, "UTF-8"); } catch (Exception e) { - responseBody = "解析响应体失败"; + responseBody = ""; } } - Log.e("WifiProbe", "HTTP 状态码: " + statusCode + ", 服务器回复: " + responseBody); + Log.e("WifiProbe", "HTTP Status Code: " + statusCode + ", Server Response: " + responseBody); callback.onError("Status: " + statusCode + " Body: " + responseBody); - } else { - // 根本没连上服务器（比如没网、超时、DNS解析失败等） - Log.e("WifiProbe", "网络底层错误: " + error.toString()); + Log.e("WifiProbe", "Network layer error: " + error.toString()); callback.onError("Network Error: " + error.toString()); } } ); - // ！！！新增：打印我们要发送给服务器的“WiFi指纹证据” ！！！ - Log.d("WifiProbe", "==== 即将发送的 WiFi 指纹: ==== \n" + jsonWifiFeatures.toString()); - // Adds the request to the request queue + + Log.d("WifiProbe", "==== WiFi Fingerprint to be sent: ==== \n" + jsonWifiFeatures.toString()); + + // Add the request to the request queue. requestQueue.add(jsonObjectRequest); } @@ -185,4 +185,4 @@ public interface VolleyCallback { void onError(String message); } -} \ No newline at end of file +} diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java index a5278af7..9b3f25ff 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java @@ -3,6 +3,7 @@ import android.util.Log; import com.google.android.gms.maps.model.LatLng; +import com.openpositioning.PositionMe.data.remote.FloorplanApiClient; import org.json.JSONException; import org.json.JSONObject; @@ -18,11 +19,13 @@ * replacing the role previously held by {@link SensorFusion}.

* * @see WifiDataProcessor the observable that triggers WiFi scan updates - * @see WiFiPositioning the API client for WiFi-based positioning + * @see WiFiPositioning the API client for WiFi-based positioning */ public class WifiPositionManager implements Observer { private static final String WIFI_FINGERPRINT = "wf"; + private static final float WIFI_FUSION_MAX_DISTANCE_METERS = 10.0f; + private static final double WIFI_FUSION_SIGMA_METERS = 12.0; private final WiFiPositioning wiFiPositioning; private final TrajectoryRecorder recorder; @@ -32,10 +35,9 @@ public class WifiPositionManager implements Observer { * Creates a new WifiPositionManager. * * @param wiFiPositioning WiFi positioning API client - * @param recorder trajectory recorder for writing WiFi fingerprints + * @param recorder trajectory recorder for writing WiFi fingerprints */ - public WifiPositionManager(WiFiPositioning wiFiPositioning, - TrajectoryRecorder recorder) { + public WifiPositionManager(WiFiPositioning wiFiPositioning, TrajectoryRecorder recorder) { this.wiFiPositioning = wiFiPositioning; this.recorder = recorder; } @@ -49,10 +51,13 @@ public WifiPositionManager(WiFiPositioning wiFiPositioning, */ @Override public void update(Object[] wifiList) { - android.util.Log.d("WifiProbe", "太好了！接收到了WiFi扫描数据！"); - this.wifiList = Stream.of(wifiList).map(o -> (Wifi) o).filter(w -> w.getLevel() < -40).collect(Collectors.toList()); + Log.d("WifiProbe", "Received WiFi scan results."); + this.wifiList = Stream.of(wifiList) + .map(o -> (Wifi) o) + .filter(w -> w.getLevel() < -40) + .collect(Collectors.toList()); recorder.addWifiFingerprint(this.wifiList); - // 改为调用带回调的方法： + // Use the callback-based request path so the fusion layer can react to the result. createWifiPositionRequestCallback(); } @@ -69,8 +74,8 @@ private void createWifiPositioningRequest() { wifiFingerPrint.put(WIFI_FINGERPRINT, wifiAccessPoints); this.wiFiPositioning.request(wifiFingerPrint); } catch (JSONException e) { - android.util.Log.e("WifiProbe", "JSON打包失败: " + e.toString()); - Log.e("jsonErrors", "Error creating json object" + e.toString()); + Log.e("WifiProbe", "Failed to build WiFi positioning JSON: " + e); + Log.e("jsonErrors", "Error creating json object" + e); } } @@ -85,61 +90,68 @@ private void createWifiPositionRequestCallback() { } JSONObject wifiFingerPrint = new JSONObject(); wifiFingerPrint.put(WIFI_FINGERPRINT, wifiAccessPoints); - android.util.Log.d("WifiProbe", "准备发送网络请求到服务器..."); + Log.d("WifiProbe", "Sending WiFi positioning request."); this.wiFiPositioning.request(wifiFingerPrint, new WiFiPositioning.VolleyCallback() { @Override public void onSuccess(LatLng wifiLocation, int floor) { - Log.d("WifiSuccess", "✅✅✅ 融合系统已接收WiFi坐标，准备更新粒子！✅✅✅"); - // --- 开始接入我们的粒子滤波器 --- - com.openpositioning.PositionMe.fusion.ParticleFilter pf = SensorFusion.getInstance().getParticleFilter(); + Log.d("WifiSuccess", "WiFi location received, updating particle filter."); - if (pf != null && wifiLocation != null) { + com.openpositioning.PositionMe.fusion.ParticleFilter pf = + SensorFusion.getInstance().getParticleFilter(); - // 1. 获取起始点 GPS 坐标作为本地坐标系的原点 (0,0) - float[] startLoc = SensorFusion.getInstance().getGNSSLatitude(true); - double lat0 = startLoc[0]; // 起点纬度 - double lng0 = startLoc[1]; // 起点经度 + if (pf != null && wifiLocation != null) { + // Use the start location as the origin of the local coordinate frame. + float[] startCoords = SensorFusion.getInstance().getGNSSLatitude(true); + double lat0 = startCoords[0]; + double lng0 = startCoords[1]; - // 2. 将 WiFi 返回的 WGS84 经纬度转换为以米为单位的局部坐标 (X, Y) - // 使用简单的平地近似公式 (Flat Earth Approximation) / 局部切面投影 - double R = 6378137.0; // 地球赤道半径（米） + // Convert the WiFi location from WGS84 to local x/y coordinates in meters. + double radius = 6378137.0; double lat = wifiLocation.latitude; double lng = wifiLocation.longitude; - - // 计算经纬度差值并转为弧度 double dLat = Math.toRadians(lat - lat0); double dLng = Math.toRadians(lng - lng0); - - // 计算 X 和 Y (米) - // X = 经度差 * 赤道半径 * cos(起点纬度) - // Y = 纬度差 * 赤道半径 - float x = (float) (R * dLng * Math.cos(Math.toRadians(lat0))); - float y = (float) (R * dLat); - - // 3. 创建 Measurement 对象 - // 假设 WiFi 定位误差约为 5.0 米 (如果没有具体数值，这里预设一个合理的 sigma) - com.openpositioning.PositionMe.fusion.Measurement m = - new com.openpositioning.PositionMe.fusion.Measurement(x, y, 10.0); - - // 4. 万事俱备！更新权重并重采样！ - pf.updateWeights(m); - pf.resample(); - - // 打印出融合后的最新位置，用于我们在控制台观察它是否生效 - com.openpositioning.PositionMe.fusion.Position pos = pf.getEstimatedPosition(); - Log.d("ParticleFilter", "融合后的平滑坐标 X: " + pos.x + " Y: " + pos.y); + float x = (float) (radius * dLng * Math.cos(Math.toRadians(lat0))); + float y = (float) (radius * dLat); + + // Build a measurement with the tuned WiFi uncertainty. + com.openpositioning.PositionMe.fusion.Measurement measurement = + new com.openpositioning.PositionMe.fusion.Measurement( + x, y, WIFI_FUSION_SIGMA_METERS); + + // Reuse the active map state so WiFi fusion respects map constraints. + LatLng startLocation = new LatLng(startCoords[0], startCoords[1]); + List walls = + SensorFusion.getInstance().getCurrentWalls(); + + SensorFusion.getInstance().setLatestWifiLocation(wifiLocation); + com.openpositioning.PositionMe.fusion.Position estimatedPosition = + pf.getEstimatedPosition(walls, startLocation); + double wifiDistance = Math.hypot( + estimatedPosition.x - x, estimatedPosition.y - y); + + if (wifiDistance <= WIFI_FUSION_MAX_DISTANCE_METERS) { + pf.updateWeights(measurement, walls, startLocation); + pf.resample(); + } else { + Log.d("WifiSuccess", + "Skipping WiFi fusion, distance too large: " + wifiDistance); + } + + com.openpositioning.PositionMe.fusion.Position pos = + pf.getEstimatedPosition(); + Log.d("ParticleFilter", + "Fused WiFi position X: " + pos.x + " Y: " + pos.y); } - // --- 粒子滤波器接入结束 --- } @Override public void onError(String message) { - // Handle the error response - android.util.Log.e("WifiProbe", "服务器报错了: " + message); + Log.e("WifiProbe", "WiFi positioning request failed: " + message); } }); } catch (JSONException e) { - Log.e("jsonErrors", "Error creating json object" + e.toString()); + Log.e("jsonErrors", "Error creating json object" + e); } } diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/GeometryUtils.java b/app/src/main/java/com/openpositioning/PositionMe/utils/GeometryUtils.java index 84d1123f..6a8c5e62 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/GeometryUtils.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/GeometryUtils.java @@ -1,4 +1,160 @@ package com.openpositioning.PositionMe.utils; +import com.google.android.gms.maps.model.LatLng; + +import java.util.List; + +/** Geometry helpers used by map matching and indoor polygon checks. */ public class GeometryUtils { + + private static final double EPSILON = 1e-9; + + /** + * Returns whether a point lies inside a polygon using ray casting. + * + * @param point point to test + * @param polygon polygon vertices + * @return {@code true} if the point lies inside the polygon + */ + public static boolean isPointInPolygon(LatLng point, List polygon) { + if (polygon == null || polygon.isEmpty()) { + return false; + } + + int intersections = 0; + for (int i = 0, j = polygon.size() - 1; i < polygon.size(); j = i++) { + LatLng p1 = polygon.get(i); + LatLng p2 = polygon.get(j); + + if (point.longitude == p1.longitude && point.latitude == p1.latitude) { + return true; + } + + if (((p1.latitude > point.latitude) != (p2.latitude > point.latitude)) + && (point.longitude + < (p2.longitude - p1.longitude) * (point.latitude - p1.latitude) + / (p2.latitude - p1.latitude) + p1.longitude)) { + intersections++; + } + } + return intersections % 2 == 1; + } + + /** + * Returns whether two line segments intersect. + * + * @param a first endpoint of the first segment + * @param b second endpoint of the first segment + * @param c first endpoint of the second segment + * @param d second endpoint of the second segment + * @return {@code true} if the segments intersect + */ + public static boolean doSegmentsIntersect(LatLng a, LatLng b, LatLng c, LatLng d) { + double d1 = direction(c, d, a); + double d2 = direction(c, d, b); + double d3 = direction(a, b, c); + double d4 = direction(a, b, d); + + if (((d1 > EPSILON && d2 < -EPSILON) || (d1 < -EPSILON && d2 > EPSILON)) + && ((d3 > EPSILON && d4 < -EPSILON) || (d3 < -EPSILON && d4 > EPSILON))) { + return true; + } + + if (Math.abs(d1) <= EPSILON && onSegment(c, d, a)) { + return true; + } + if (Math.abs(d2) <= EPSILON && onSegment(c, d, b)) { + return true; + } + if (Math.abs(d3) <= EPSILON && onSegment(a, b, c)) { + return true; + } + if (Math.abs(d4) <= EPSILON && onSegment(a, b, d)) { + return true; + } + + return false; + } + + private static double direction(LatLng a, LatLng b, LatLng c) { + return (c.longitude - a.longitude) * (b.latitude - a.latitude) + - (b.longitude - a.longitude) * (c.latitude - a.latitude); + } + + private static boolean onSegment(LatLng a, LatLng b, LatLng p) { + return p.longitude <= Math.max(a.longitude, b.longitude) + EPSILON + && p.longitude >= Math.min(a.longitude, b.longitude) - EPSILON + && p.latitude <= Math.max(a.latitude, b.latitude) + EPSILON + && p.latitude >= Math.min(a.latitude, b.latitude) - EPSILON; + } + + /** + * Returns the planar distance between two geographic points in meters. + * + * @param a first point + * @param b second point + * @return approximate distance in meters + */ + public static double distanceMeters(LatLng a, LatLng b) { + double radius = 6378137.0; + double lat1 = Math.toRadians(a.latitude); + double lat2 = Math.toRadians(b.latitude); + double dLat = lat2 - lat1; + double dLng = Math.toRadians(b.longitude - a.longitude); + double x = dLng * Math.cos((lat1 + lat2) / 2.0); + double y = dLat; + return Math.sqrt(x * x + y * y) * radius; + } + + /** + * Returns the minimum distance between a point and a polygon in meters. + * + * @param point point to test + * @param polygon polygon vertices + * @return distance in meters, or {@link Double#POSITIVE_INFINITY} for an empty polygon + */ + public static double distancePointToPolygonMeters(LatLng point, List polygon) { + if (polygon == null || polygon.isEmpty()) { + return Double.POSITIVE_INFINITY; + } + if (polygon.size() >= 3 && isPointInPolygon(point, polygon)) { + return 0.0; + } + + double minDistance = Double.POSITIVE_INFINITY; + for (int i = 0; i < polygon.size(); i++) { + LatLng a = polygon.get(i); + LatLng b = polygon.get((i + 1) % polygon.size()); + minDistance = Math.min(minDistance, distancePointToSegmentMeters(point, a, b)); + } + return minDistance; + } + + private static double distancePointToSegmentMeters(LatLng point, LatLng a, LatLng b) { + double latScale = 111320.0; + double lngScale = + Math.cos(Math.toRadians((a.latitude + b.latitude + point.latitude) / 3.0)) * 111320.0; + + double px = point.longitude * lngScale; + double py = point.latitude * latScale; + double ax = a.longitude * lngScale; + double ay = a.latitude * latScale; + double bx = b.longitude * lngScale; + double by = b.latitude * latScale; + + double abx = bx - ax; + double aby = by - ay; + double apx = px - ax; + double apy = py - ay; + double lengthSquared = abx * abx + aby * aby; + + double t = lengthSquared <= EPSILON ? 0.0 : (apx * abx + apy * aby) / lengthSquared; + t = Math.max(0.0, Math.min(1.0, t)); + + double closestX = ax + t * abx; + double closestY = ay + t * aby; + double dx = px - closestX; + double dy = py - closestY; + return Math.sqrt(dx * dx + dy * dy); + } } diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java index 48fc83c1..93032390 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java @@ -164,12 +164,8 @@ public void setCurrentFloor(int newFloor, boolean autoFloor) { int targetIndex = -1; if (autoFloor) { - // 如果是自动切换（来自WiFi/气压计的数字 0, 1, -1） - // 现在的列表顺序很整齐：-1对应0, 0对应1, 1对应2... - // 规律就是：索引 = 物理楼层 + 1 targetIndex = newFloor + 1; } else { - // 如果是手动按钮传入的，它传的通常就是目标 index targetIndex = newFloor; } @@ -177,6 +173,10 @@ public void setCurrentFloor(int newFloor, boolean autoFloor) { this.currentFloor = targetIndex; drawFloorShapes(targetIndex); } + List currentFeatures = currentFloorShapes.get(targetIndex).getFeatures(); + SensorFusion.getInstance().setCurrentWalls(currentFeatures); + + android.util.Log.d("FloorFix", "鎴愬姛鍒囨崲鍒版ゼ灞傜储寮? " + targetIndex + "锛屽苟鏇存柊浜嗗澹佹暟鎹紒"); } /** @@ -242,6 +242,8 @@ private void setBuildingOverlay() { if (currentFloorShapes != null && !currentFloorShapes.isEmpty()) { drawFloorShapes(currentFloor); isIndoorMapSet = true; + List currentFeatures = currentFloorShapes.get(currentFloor).getFeatures(); + SensorFusion.getInstance().setCurrentWalls(currentFeatures); } } else if (!inAnyBuilding && isIndoorMapSet) { @@ -418,4 +420,12 @@ public void setIndicationOfIndoorMap() { gMap.addPolyline(new PolylineOptions().color(Color.GREEN).addAll(pts)); } } + /** + */ + public List getCurrentFloorMapShapes() { + if (currentFloorShapes != null && currentFloor >= 0 && currentFloor < currentFloorShapes.size()) { + return currentFloorShapes.get(currentFloor).getFeatures(); + } + return null; + } } diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java index 385d0d9b..39811ba3 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java @@ -38,6 +38,12 @@ public class PdrProcessing { // Threshold under which movement is considered non-existent private static final float epsilon = 0.18f; private static final int MIN_REQUIRED_SAMPLES = 2; + private static final float HEADING_MEDIUM_JUMP_THRESHOLD_RAD = (float) Math.toRadians(32.0); + private static final float HEADING_JUMP_THRESHOLD_RAD = (float) Math.toRadians(50.0); + private static final float HEADING_SMOOTH_ALPHA_VERY_STABLE = 0.08f; + private static final float HEADING_SMOOTH_ALPHA_STABLE = 0.25f; + private static final float HEADING_SMOOTH_ALPHA_TURNING = 0.80f; + private static final float GYRO_TURN_CONFIRM_THRESHOLD_RAD_S = 0.6f; //endregion //region Instance variables @@ -71,6 +77,7 @@ public class PdrProcessing { // Step sum and length aggregation variables private float sumStepLength = 0; private int stepCount = 0; + private float filteredHeadingRad = Float.NaN; //endregion /** @@ -84,15 +91,15 @@ public PdrProcessing(Context context) { this.settings = PreferenceManager.getDefaultSharedPreferences(context); // // Check if estimate or manual values should be used // this.useManualStep = this.settings.getBoolean("manual_step_values", false); - // 强制开启手动步长模式，不再使用不准的自动估计算法！ + // Keep manual step length enabled because it is more stable than the auto estimate here. this.useManualStep = true; if(useManualStep) { try { // Retrieve manual step length - this.stepLength = this.settings.getInt("user_step_length", 20) / 100f; + this.stepLength = this.settings.getInt("user_step_length", 75) / 100f; } catch (Exception e) { // Invalid values - reset to defaults - this.stepLength = 0.20f; + this.stepLength = 0.75f; this.settings.edit().putInt("user_step_length", 20).apply(); } } @@ -142,13 +149,23 @@ public PdrProcessing(Context context) { * @param headingRad heading relative to magnetic north in radians. */ public float[] updatePdr(long currentStepEnd, List accelMagnitudeOvertime, float headingRad) { + return updatePdr(currentStepEnd, accelMagnitudeOvertime, headingRad, 0f, false); + } + + public float[] updatePdr(long currentStepEnd, + List accelMagnitudeOvertime, + float headingRad, + float gyroZRadPerSec, + boolean recentTurnDetected) { if (accelMagnitudeOvertime == null || accelMagnitudeOvertime.size() < MIN_REQUIRED_SAMPLES) { return new float[]{this.positionX, this.positionY}; // Return current position without update - // - TODO - temporary solution of the empty list issue + // - TODO - temporary solution of the empty list issue } + float stableHeading = stabiliseHeading(headingRad, gyroZRadPerSec, recentTurnDetected); + // Change angle so zero rad is east - float adaptedHeading = (float) (Math.PI/2 - headingRad); + float adaptedHeading = (float) (Math.PI/2 - stableHeading); // check if accelMagnitudeOvertime is empty if (accelMagnitudeOvertime == null || accelMagnitudeOvertime.isEmpty()) { @@ -163,9 +180,8 @@ public float[] updatePdr(long currentStepEnd, List accelMagnitudeOvertim this.stepLength = weibergMinMax(accelMagnitudeOvertime); // System.err.println("Step Length" + stepLength); } - // ！！！强制修正：无视所有前面的算法，步长就按我们说的算！！！ - // ！！！我们先从一个极小的值开始测试，比如 30 厘米 ！！！ - stepLength = 0.70f; + // Use the manually tuned constant step length after testing. + stepLength = 0.75f; // Increment aggregate variables sumStepLength += stepLength; @@ -183,6 +199,54 @@ public float[] updatePdr(long currentStepEnd, List accelMagnitudeOvertim return new float[]{this.positionX, this.positionY}; } + private float stabiliseHeading(float rawHeadingRad, float gyroZRadPerSec, boolean recentTurnDetected) { + if (Float.isNaN(filteredHeadingRad)) { + filteredHeadingRad = normalizeAngle(rawHeadingRad); + return filteredHeadingRad; + } + + float normalizedHeading = unwrapAngle(rawHeadingRad, filteredHeadingRad); + float delta = normalizedHeading - filteredHeadingRad; + float absDelta = Math.abs(delta); + boolean mediumJump = absDelta > HEADING_MEDIUM_JUMP_THRESHOLD_RAD; + boolean largeJump = Math.abs(delta) > HEADING_JUMP_THRESHOLD_RAD; + boolean activeTurn = recentTurnDetected || Math.abs(gyroZRadPerSec) >= GYRO_TURN_CONFIRM_THRESHOLD_RAD_S; + + if (largeJump && !activeTurn) { + return filteredHeadingRad; + } + + if (mediumJump && !activeTurn) { + filteredHeadingRad = normalizeAngle(filteredHeadingRad + HEADING_SMOOTH_ALPHA_VERY_STABLE * delta); + return filteredHeadingRad; + } + + float alpha = activeTurn ? HEADING_SMOOTH_ALPHA_TURNING : HEADING_SMOOTH_ALPHA_STABLE; + filteredHeadingRad = normalizeAngle(filteredHeadingRad + alpha * delta); + return filteredHeadingRad; + } + + private float unwrapAngle(float angle, float referenceAngle) { + float normalized = normalizeAngle(angle); + while (normalized - referenceAngle > Math.PI) { + normalized -= (float) (2 * Math.PI); + } + while (normalized - referenceAngle < -Math.PI) { + normalized += (float) (2 * Math.PI); + } + return normalized; + } + + private float normalizeAngle(float angle) { + while (angle > Math.PI) { + angle -= (float) (2 * Math.PI); + } + while (angle < -Math.PI) { + angle += (float) (2 * Math.PI); + } + return angle; + } + /** * Calculates the relative elevation compared to the start position. * The start elevation is the median of the first three seconds of data to give the sensor time @@ -400,6 +464,7 @@ public void resetPDR() { this.startElevationBuffer = new Float[3]; // Start floor - assumed to be zero this.currentFloor = 0; + this.filteredHeadingRad = Float.NaN; } /**