feat(q3): 贝叶斯网格粒子滤波闭环(PF负观测+信息增益选点+2-opt调度)
- particles.py: 网格幸存集滤波,Voronoi 裕量保证真源保留(缺席证明/网格清除证书/跳测规则)
- planner/bayes_loop.py: 统一任务循环(站点/定位/清除滚动 2-opt 排序)、期望后验半径选点
- planner/seven.py: 七点覆盖策略族提升为库代码
- cli: mcm26b run --strategy {grid,seven,bayes}
- BayesConfig 参数组;tests +24 项(真源保留/证书/端到端),129 全绿
This commit is contained in:
@@ -0,0 +1,80 @@
|
|||||||
|
# 目标
|
||||||
|
|
||||||
|
在新分支 **`q3-bayes`**(基于当前 `gpt`)实现第三问新算法:**网格化贝叶斯粒子滤波(含负观测更新)+ 信息增益选点 + 开放路径 2-opt 任务排序 + 七点覆盖兜底 + Optuna 离线调参**。评分口径 = 虚拟总时间(A2 基线:官方演练 12 源 5232s、109 次检测);硬性要求全清率 100%、缺席证明正确、零异常。
|
||||||
|
|
||||||
|
# 关键设计
|
||||||
|
|
||||||
|
## 1. 粒子滤波 `src/mcm26b/particles.py`(新建)
|
||||||
|
|
||||||
|
与用户方案的差异点:本项目要求清除证书/缺席证明**严格**(A2 靠保守几何证书做到 100% 清除率),软核概率 PF 有删掉真源的风险。因此采用**网格幸存集滤波**(= 硬似然 PF + Voronoi 裕量,与用户"|θ̂−θ|>1° 权重置零"的设定一致但更严格):
|
||||||
|
|
||||||
|
- 每频道一组 Ω 上间距 h≈30m 的网格粒子(约 1.1 万格,alive 用整数索引集合;空间哈希加速邻域查询)
|
||||||
|
- 保守 kill 判定(margin = h·√2/2,保证真源所在格必活):
|
||||||
|
- `no_signal`@s:kill dist(g,s) ≤ 1000 − margin(题面 r_eff ≥ 1000 ⟹ 可严格排除该盘)
|
||||||
|
- `direction`@s 读数 θ:kill 角向偏差超出 δ_eff=1.005°(含余量)或 dist > 1500 + margin 的格
|
||||||
|
- `near`@s:kill dist(g,s) > 5 + margin
|
||||||
|
- 证书全部由此推导:清除证书 = MEC(alive).R + margin ≤ 20 − clear_margin;**缺席证明 = alive 为空集**(无需额外像素覆盖检查);跳测规则 = min dist(alive, s) > 1500 + margin ⟹ 必 no_signal,跳过省 6s/次
|
||||||
|
- 信息增益:对候选点用子采样(≤500 粒子)× K 个代表假设做 MC 期望熵减估计;复用 `smallest_enclosing_circle`(Welzl) 求 MEC
|
||||||
|
|
||||||
|
## 2. 新策略 `BayesLoop(OmnidirectionalLoop)`(`src/mcm26b/planner/bayes_loop.py`)
|
||||||
|
|
||||||
|
复用基类 run() 骨架、异常收尾、summary、anomaly、预算机制,重写阶段逻辑:
|
||||||
|
- **发现阶段**:原点起扫(频道排序省换频)→ 站点 = 七点环 ∪ PF 高价值点,按"期望发现数 − λ·移动时间"贪心选站;每站批量扫频时跳过"PF 判定必 no_signal"的 (站,频道) 对与已证缺席频道;每站扫完即服务已发现频道(SevenBatch 风格)
|
||||||
|
- **定位服务**:选点 = 保证接收盘内候选环采样 + 信息增益评分(ΔH − λ·移动 − μ·检测),有证书即清除;步数上限沿用 `max_localize_steps`
|
||||||
|
- **清除调度**:待服务任务(定位+清除 ≤20 节点)开放路径最近邻 + 2-opt,纯标准库,每个决策点滚动重排
|
||||||
|
- **收尾**:全部 CLEARED/ABSENT → all_resolved;清满 16 提前停;persistent_search 沿用现有配置语义(实现时对照 `run_q3_seven_practice.py` 保持官方演练停止行为一致)
|
||||||
|
|
||||||
|
## 3. 配置 `config.py`
|
||||||
|
|
||||||
|
新增 `BayesConfig` frozen dataclass([工程]):particle_spacing_m、λ/μ 权重、MC 代表数/子采样、skip_margin、站点环半径、2-opt 开关/迭代数等;挂到 `ProblemConfig.bayes`。默认值不改变现有策略行为。
|
||||||
|
|
||||||
|
## 4. 七点覆盖提升为库代码 `src/mcm26b/planner/seven.py`
|
||||||
|
|
||||||
|
把 `scripts/benchmark_q3_search.py` 的 `seven_points()/SevenImmediate/SevenBatch` 移入 src(旧脚本改 import,消融结果可复现),供 CLI 与新基准复用。
|
||||||
|
|
||||||
|
## 5. CLI `cli.py`
|
||||||
|
|
||||||
|
`mcm26b run --strategy {grid,seven,bayes}`,默认 grid;**若基准通过则默认改为 bayes**(用户已确认)。
|
||||||
|
|
||||||
|
## 6. 基准 `scripts/benchmark_q3_bayes.py`
|
||||||
|
|
||||||
|
配对消融 {seven_batch, bayes} ×(seeds 0..5 × 10/16 源 + 边界源 + 0 源纯缺席场景),复用现有 run_case/LocalSimulator 模式;输出 `output/q3/bayes-ablation/`(benchmark.json + report.md):全清率、correct_absence、异常、accounted/total 虚拟时间、配对降幅、实机 compute_wall_s。
|
||||||
|
|
||||||
|
## 7. Optuna 调参 `scripts/tune_q3_bayes.py`
|
||||||
|
|
||||||
|
- `uv add --group dev optuna`(仅调参脚本用,生产零新增依赖)
|
||||||
|
- TPE,目标 = 场景子集平均虚拟时间 + 10⁶×失败惩罚;场景与 error_seed 多样化防过拟合
|
||||||
|
- 搜索空间:h、λ/μ、skip margin、候选环、2-opt 开关等
|
||||||
|
- optuna 不可用时自动退化为随机搜索;最优参数写 JSON,复跑基准确认后合入 config.py 默认值
|
||||||
|
|
||||||
|
## 8. 测试
|
||||||
|
|
||||||
|
- `tests/test_particles.py`:真源保留性(随机场景 × LocalSimulator 误差场,任意更新序列后真源邻域格必活);证书正确性(alive=∅ ⟺ 真缺席;MEC 证书清除必成功);跳测规则永不漏检
|
||||||
|
- `tests/test_bayes_loop_q3.py`:端到端 LocalSimulator(10/16/0 源、边界源、预算耗尽、故障注入):全清 + 缺席正确 + 无异常 + 正常退出
|
||||||
|
- 现有 105 项测试保持绿
|
||||||
|
|
||||||
|
## 9. 文档
|
||||||
|
|
||||||
|
`docs/algo-3-bayes.md`(Voronoi 裕量严格性论证、信息增益与调度设计、与 A2 对比数据);README 策略表加行;`docs/algo-3.md` 加交叉引用。
|
||||||
|
|
||||||
|
# 执行顺序
|
||||||
|
|
||||||
|
1. `git checkout -b q3-bayes`
|
||||||
|
2. BayesConfig → particles.py(含单测)→ seven.py 提升
|
||||||
|
3. bayes_loop.py + CLI 接入 → pytest 全绿
|
||||||
|
4. 跑配对消融基准
|
||||||
|
5. Optuna 调参 → 回填参数复跑基准
|
||||||
|
6. 若达标(全清 100% 且配对均值更优):`mcm26b run` 默认切 bayes + 文档更新
|
||||||
|
7. 分支上分阶段 commit(不 push)
|
||||||
|
|
||||||
|
# 验收标准
|
||||||
|
|
||||||
|
- 基准全部场景:all_cleared 100%、correct_absence 100%、anomalies 空、normal_exit、单局实机计算 < 60s
|
||||||
|
- 配对平均虚拟时间优于 seven_batch(目标降幅 ≥5%;参考量级 A2=5232s/12 源)
|
||||||
|
- `uv run pytest` 全绿;`mcm26b run --self-test --problem 3` 通过
|
||||||
|
|
||||||
|
# 风险与对策
|
||||||
|
|
||||||
|
- PF 实机计算耗时 → 空间哈希 + MC 子采样,基准记录 compute_wall_s,超预算先降 MC 规模(实机预算 1200s,A2 仅用 3.4s,余量巨大)
|
||||||
|
- LocalSimulator 误差场确定性 → 调参与基准用多 error_seed
|
||||||
|
- 负观测删点的正确性 → Voronoi 裕量定理 + 随机化真源保留性测试兜底
|
||||||
@@ -14,80 +14,20 @@ from pathlib import Path
|
|||||||
from statistics import mean, median
|
from statistics import mean, median
|
||||||
from time import perf_counter
|
from time import perf_counter
|
||||||
|
|
||||||
from mcm26b.belief import ChannelStatus
|
from mcm26b.belief import ChannelStatus # noqa: F401 (兼容旧 import 路径)
|
||||||
from mcm26b.config import ProblemConfig
|
from mcm26b.config import ProblemConfig
|
||||||
from mcm26b.coverage import CoverageLedger
|
from mcm26b.coverage import CoverageLedger # noqa: F401
|
||||||
from mcm26b.planner import OmnidirectionalLoop, clear_certificate
|
from mcm26b.planner import OmnidirectionalLoop, clear_certificate # noqa: F401
|
||||||
|
from mcm26b.planner.seven import (SevenBatch, SevenImmediate, SevenSweep,
|
||||||
|
RING_RADIUS_M, seven_points)
|
||||||
from mcm26b.protocol.client import SimulatorClient
|
from mcm26b.protocol.client import SimulatorClient
|
||||||
from mcm26b.protocol.rules import NEAR
|
from mcm26b.protocol.rules import NEAR # noqa: F401
|
||||||
from mcm26b.sim import LocalSimulator, LocalSimTransport, Source
|
from mcm26b.sim import LocalSimulator, LocalSimTransport, Source
|
||||||
from mcm26b.sim.scenes import random_omni_scene
|
from mcm26b.sim.scenes import random_omni_scene
|
||||||
|
|
||||||
RING_RADIUS_M = 1200.0 # Engineering choice; analytic proof in algo-3 section 5.
|
|
||||||
ROOT = Path(__file__).resolve().parents[1]
|
ROOT = Path(__file__).resolve().parents[1]
|
||||||
|
|
||||||
|
|
||||||
def seven_points():
|
|
||||||
return [(0.0, 0.0)] + [(RING_RADIUS_M * math.cos(k * math.pi / 3),
|
|
||||||
RING_RADIUS_M * math.sin(k * math.pi / 3)) for k in range(6)]
|
|
||||||
|
|
||||||
|
|
||||||
class SevenImmediate(OmnidirectionalLoop):
|
|
||||||
def __init__(self, client, config):
|
|
||||||
super().__init__(client, config)
|
|
||||||
self.ledger = CoverageLedger(config=config, points=seven_points())
|
|
||||||
|
|
||||||
|
|
||||||
class SevenBatch(SevenImmediate):
|
|
||||||
"""Finish scanning a station, then serve detected sources in nearest order."""
|
|
||||||
defer_all = False
|
|
||||||
|
|
||||||
def _serve_queue(self):
|
|
||||||
# Each detected channel gets at most one localization call in this pass.
|
|
||||||
queue = self.belief.channels_by_status(ChannelStatus.DETECTED)
|
|
||||||
while queue:
|
|
||||||
c = min(queue, key=lambda c: (math.dist(self.client.state.position,
|
|
||||||
self.belief.candidate_set(c).mec()[0]), c))
|
|
||||||
queue.remove(c)
|
|
||||||
self._localize_and_clear(c)
|
|
||||||
if self._count_limit_reached():
|
|
||||||
break
|
|
||||||
|
|
||||||
def _coverage_phase(self):
|
|
||||||
while not self._count_limit_reached():
|
|
||||||
index = self._next_point_index()
|
|
||||||
if index is None:
|
|
||||||
return
|
|
||||||
point = self.ledger.points[index]
|
|
||||||
pending = self._pending_channels_at(index)
|
|
||||||
# Keeping the current measuring channel first saves a switch when possible.
|
|
||||||
pending.sort(key=lambda c: (c != self.client.state.channel, c))
|
|
||||||
for c in pending:
|
|
||||||
outcome = self.client.measure(point, c)
|
|
||||||
self.ledger.mark_measured(c, index)
|
|
||||||
self.belief.update_measure(c, point, outcome.result, outcome.svd_deg)
|
|
||||||
if outcome.result == NEAR:
|
|
||||||
cert = clear_certificate(self.belief.candidate_set(c), c, self.config)
|
|
||||||
# Same-station near clear; do not displace the station scan.
|
|
||||||
if cert.certified:
|
|
||||||
self._attempt_clear(c, point, cert)
|
|
||||||
if self._count_limit_reached():
|
|
||||||
return
|
|
||||||
if not self.defer_all:
|
|
||||||
self._serve_queue()
|
|
||||||
|
|
||||||
def _resolve_detected(self):
|
|
||||||
self._serve_queue()
|
|
||||||
unresolved = [c for c, b in self.belief.channels.items()
|
|
||||||
if b.status not in (ChannelStatus.CLEARED, ChannelStatus.ABSENT)]
|
|
||||||
return "all_resolved" if not unresolved else "unresolved_channels"
|
|
||||||
|
|
||||||
|
|
||||||
class SevenSweep(SevenBatch):
|
|
||||||
"""Finish discovery at all necessary stations before servicing the queue."""
|
|
||||||
defer_all = True
|
|
||||||
|
|
||||||
|
|
||||||
VARIANTS = {"grid49_immediate": OmnidirectionalLoop, "seven_immediate": SevenImmediate,
|
VARIANTS = {"grid49_immediate": OmnidirectionalLoop, "seven_immediate": SevenImmediate,
|
||||||
"seven_batch": SevenBatch, "seven_sweep": SevenSweep}
|
"seven_batch": SevenBatch, "seven_sweep": SevenSweep}
|
||||||
|
|
||||||
|
|||||||
+12
-1
@@ -17,12 +17,16 @@ from pathlib import Path
|
|||||||
from .config import ProblemConfig
|
from .config import ProblemConfig
|
||||||
from .geometry.__main__ import main as q1_main
|
from .geometry.__main__ import main as q1_main
|
||||||
from .planner import DirectionalExtension, OmnidirectionalLoop
|
from .planner import DirectionalExtension, OmnidirectionalLoop
|
||||||
|
from .planner.bayes_loop import BayesLoop
|
||||||
|
from .planner.seven import SevenBatch
|
||||||
from .protocol.client import SimulatorClient
|
from .protocol.client import SimulatorClient
|
||||||
from .protocol.log import SessionLog
|
from .protocol.log import SessionLog
|
||||||
from .protocol.rules import EXIT_USER
|
from .protocol.rules import EXIT_USER
|
||||||
from .sim import LocalSimTransport, LocalSimulator, Source
|
from .sim import LocalSimTransport, LocalSimulator, Source
|
||||||
from .sim.scenes import mixed_scene, random_omni_scene
|
from .sim.scenes import mixed_scene, random_omni_scene
|
||||||
|
|
||||||
|
Q3_STRATEGIES = {"grid": OmnidirectionalLoop, "seven": SevenBatch, "bayes": BayesLoop}
|
||||||
|
|
||||||
|
|
||||||
def _sim_check(args) -> int:
|
def _sim_check(args) -> int:
|
||||||
"""附件1表2/附件2第10节计时示例:期望 105/111/194/199 秒。"""
|
"""附件1表2/附件2第10节计时示例:期望 105/111/194/199 秒。"""
|
||||||
@@ -83,9 +87,13 @@ def _run(args) -> int:
|
|||||||
"/enter 返回的 remaining_real_duration_s 为准。", file=sys.stderr)
|
"/enter 返回的 remaining_real_duration_s 为准。", file=sys.stderr)
|
||||||
log = SessionLog(path=Path(args.log)) if args.log else SessionLog()
|
log = SessionLog(path=Path(args.log)) if args.log else SessionLog()
|
||||||
client = SimulatorClient(args.robot_id, config=config, transport=transport, log=log)
|
client = SimulatorClient(args.robot_id, config=config, transport=transport, log=log)
|
||||||
loop_cls = DirectionalExtension if args.problem == 4 else OmnidirectionalLoop
|
if args.problem == 4:
|
||||||
|
loop_cls = DirectionalExtension
|
||||||
|
else:
|
||||||
|
loop_cls = Q3_STRATEGIES[args.strategy]
|
||||||
summary = loop_cls(client, config).run()
|
summary = loop_cls(client, config).run()
|
||||||
summary["robot_id"] = args.robot_id
|
summary["robot_id"] = args.robot_id
|
||||||
|
summary["strategy"] = ("directional" if args.problem == 4 else args.strategy)
|
||||||
if real_budget_s is not None:
|
if real_budget_s is not None:
|
||||||
summary["self_test_real_budget_s"] = real_budget_s
|
summary["self_test_real_budget_s"] = real_budget_s
|
||||||
if args.summary_steps is not None and len(summary.get("steps", [])) > args.summary_steps:
|
if args.summary_steps is not None and len(summary.get("steps", [])) > args.summary_steps:
|
||||||
@@ -125,6 +133,9 @@ def main(argv=None) -> int:
|
|||||||
run.add_argument("--log", default=None, help="会话日志 JSONL 输出路径")
|
run.add_argument("--log", default=None, help="会话日志 JSONL 输出路径")
|
||||||
run.add_argument("--problem", type=int, choices=(3, 4), default=3,
|
run.add_argument("--problem", type=int, choices=(3, 4), default=3,
|
||||||
help="问题3=全向源,问题4=混合定向源(默认 3)")
|
help="问题3=全向源,问题4=混合定向源(默认 3)")
|
||||||
|
run.add_argument("--strategy", choices=tuple(Q3_STRATEGIES), default="grid",
|
||||||
|
help="问题3策略:grid=49点网格基线,seven=七点批量,"
|
||||||
|
"bayes=粒子滤波+信息增益(默认 grid;仅 --problem 3 生效)")
|
||||||
run.add_argument("--bearing-margin-deg", type=float, default=0.005,
|
run.add_argument("--bearing-margin-deg", type=float, default=0.005,
|
||||||
help="示向度舍入裕量(服务器保留两位小数;默认 0.005°)")
|
help="示向度舍入裕量(服务器保留两位小数;默认 0.005°)")
|
||||||
run.add_argument("--real-budget-s", type=float, default=None,
|
run.add_argument("--real-budget-s", type=float, default=None,
|
||||||
|
|||||||
@@ -99,6 +99,38 @@ class SearchConfig:
|
|||||||
# 避免触及 100 小时上限被模拟器强制结束
|
# 避免触及 100 小时上限被模拟器强制结束
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class BayesConfig:
|
||||||
|
"""P4b 贝叶斯网格滤波策略参数(问题3新策略 bayes)。[工程]
|
||||||
|
|
||||||
|
网格粒子滤波的严格性依赖 Voronoi 裕量:真实源所在格心与源的距离
|
||||||
|
<= sqrt(2)/2 * particle_spacing_m,所有 kill 判定都内缩该裕量,
|
||||||
|
因此幸存集永远包含真源(定理见 docs/algo-3-bayes.md §2)。
|
||||||
|
"""
|
||||||
|
|
||||||
|
particle_spacing_m: float = 25.0 # 网格间距;Voronoi 裕量 ≈ 17.68m;
|
||||||
|
# 全域约 1.6 万格/频道,20 频道内存与耗时可控
|
||||||
|
# ---- 信息增益评分 [工程] ----
|
||||||
|
score_mode: str = "radius" # radius=期望后验半径(直指证书)|
|
||||||
|
# entropy=期望 log 幸存占比(信息增益)
|
||||||
|
score_subsample: int = 800 # 期望幸存熵估计的粒子子采样规模
|
||||||
|
mc_representatives: int = 6 # 假设真值代表点数(均匀抽取幸存格)
|
||||||
|
lambda_move_s: float = 0.1 # 选点评分中移动时间的折算系数(分值/秒);
|
||||||
|
# 与后验半径同量纲,量级须小否则终局拒走(欠收敛)
|
||||||
|
wide_rings: tuple = (1.0, 1.5, 2.0) # 无保证接收盘时的探索环(× PF MEC 半径)
|
||||||
|
# ---- 站点选择 [工程] ----
|
||||||
|
station_ring_radius_m: float = 1200.0 # 七点环半径;覆盖半径 968.9m 的解析证明
|
||||||
|
inner_mass_weight: float = 0.5 # 站点评分中 1000m 内幸存质量的权重
|
||||||
|
resolve_bonus: float = 10.0 # "就地可证毕"(no_signal 即缺席/有信号即收敛)
|
||||||
|
# 频道的站点评分加成
|
||||||
|
station_move_penalty_s: float = 40.0 # 站点评分中移动时间(秒)的折算分母
|
||||||
|
# ---- 任务排序 [工程] ----
|
||||||
|
two_opt_enabled: bool = True # 开放路径 2-opt 改进(最近邻打底)
|
||||||
|
two_opt_passes: int = 3 # 2-opt 最大改进轮数
|
||||||
|
# ---- PF 清除证书 [工程] ----
|
||||||
|
grid_cert_enabled: bool = True # 允许用幸存集 MEC 颁发清除证书(凸证书之外)
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
class ProblemConfig:
|
class ProblemConfig:
|
||||||
"""聚合配置。各模块函数的第一个默认参数,便于单测替换。"""
|
"""聚合配置。各模块函数的第一个默认参数,便于单测替换。"""
|
||||||
@@ -107,6 +139,7 @@ class ProblemConfig:
|
|||||||
cost: CostConfig = CostConfig()
|
cost: CostConfig = CostConfig()
|
||||||
session: SessionConfig = SessionConfig()
|
session: SessionConfig = SessionConfig()
|
||||||
search: SearchConfig = SearchConfig()
|
search: SearchConfig = SearchConfig()
|
||||||
|
bayes: BayesConfig = BayesConfig()
|
||||||
|
|
||||||
|
|
||||||
DEFAULT_CONFIG = ProblemConfig()
|
DEFAULT_CONFIG = ProblemConfig()
|
||||||
|
|||||||
@@ -0,0 +1,356 @@
|
|||||||
|
"""P4b 贝叶斯网格粒子滤波:逐频道幸存集维护与负观测更新。
|
||||||
|
|
||||||
|
与 belief.CandidateRegion(保守凸外包,无法表达非凸"挖洞")互补:
|
||||||
|
网格幸存集精确表达"角域 ∩ 距离盘 ∩ 全部 no_signal 排除盘"的非凸可行集,
|
||||||
|
从而支持三件凸表示做不到的事——
|
||||||
|
|
||||||
|
1. 缺席证明:幸存集为空 ⟹ 可行集为空 ⟹ 该频道无源(严格,无需覆盖账本);
|
||||||
|
2. 跳测规则:幸存格到测点的最小距离 > r_max + 裕量 ⟹ 该次测量必 no_signal,
|
||||||
|
且不产生任何新排除,可直接跳过(省 5s 检测 + 1s 换频);
|
||||||
|
3. 负观测更新:no_signal 排除 B(s, 1000),凸表示不删点,网格直接删格。
|
||||||
|
|
||||||
|
严格性(真源保留定理,docs/algo-3-bayes.md §2):格距 h 的 Voronoi 裕量
|
||||||
|
m = sqrt(2)/2·h 是格心到格内任一点的最大距离。所有 kill 判定均内缩 m,
|
||||||
|
即"仅当整格与可行集不相交时才删格"。真源所在格与可行集之交含真源本身,
|
||||||
|
故该格永不被删——任意观测序列后幸存集恒包含真源,且:
|
||||||
|
- 清除证书:MEC(幸存).R + m <= 20 − 工程余量 ⟹ 真源距清除点 <= 20;
|
||||||
|
- 缺席证明:幸存集为空 ⟹ 可行集为空。
|
||||||
|
|
||||||
|
观测语义与 sim.Simulator 一致 [题面]:
|
||||||
|
- no_signal:距离 > r_eff >= 1000 ⟹ 可行集 ∩ B(s,1000) = ∅;
|
||||||
|
- direction:距离 <= 1500,示向度误差 |读数−真值| <= δ_eff(含舍入裕量);
|
||||||
|
- near:距离 <= 5。
|
||||||
|
|
||||||
|
性能设计:kill 与查询全部走 O(1) 集合运算;最近距离查询用粗粒度占用
|
||||||
|
索引(每格聚合 CF² 个细格)螺旋扫描 + 早退,服务跳测规则的高频调用。
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import math
|
||||||
|
|
||||||
|
from .config import ProblemConfig, Vec2
|
||||||
|
from .geometry.localization import smallest_enclosing_circle
|
||||||
|
from .protocol.rules import DIRECTION, NEAR, NO_SIGNAL
|
||||||
|
|
||||||
|
_ANG_EPS_DEG = 1e-9 # 角向 kill 比较的浮点余量
|
||||||
|
_DIST_EPS_M = 1e-9
|
||||||
|
_COARSE_FACTOR = 4 # 粗格每边聚合的细格数
|
||||||
|
|
||||||
|
|
||||||
|
def angular_gap(a_deg: float, b_deg: float) -> float:
|
||||||
|
"""两个方位角(度)的最小夹角。"""
|
||||||
|
return abs((a_deg - b_deg + 180.0) % 360.0 - 180.0)
|
||||||
|
|
||||||
|
|
||||||
|
class ChannelParticles:
|
||||||
|
"""单个频道的网格幸存集。
|
||||||
|
|
||||||
|
网格为覆盖 Ω 外扩一格的轴对齐方格(边长 h),索引 ``i*W + j``;
|
||||||
|
``alive`` 是幸存格索引集合。初始集 = 格心距原点 <= Ω 半径 + 裕量的
|
||||||
|
全部格子,因此真源(|p| <= 1800 [题面])所在格必在初始集内。
|
||||||
|
``version`` 随每次删除自增,调用方可据此缓存派生量(MEC、跳测判定)。
|
||||||
|
"""
|
||||||
|
|
||||||
|
def __init__(self, config: ProblemConfig = ProblemConfig(),
|
||||||
|
spacing_m: float | None = None):
|
||||||
|
field = config.field
|
||||||
|
self.config = config
|
||||||
|
self.spacing_m = float(spacing_m if spacing_m is not None
|
||||||
|
else config.bayes.particle_spacing_m)
|
||||||
|
if not 1.0 <= self.spacing_m <= 200.0:
|
||||||
|
raise ValueError("网格间距须在 [1, 200] 米")
|
||||||
|
self.margin_m = self.spacing_m * math.sqrt(2) / 2 # Voronoi 裕量
|
||||||
|
self.r_eff_min_m = field.r_eff_min_m
|
||||||
|
self.r_eff_max_m = field.r_eff_max_m
|
||||||
|
self.near_radius_m = field.near_radius_m
|
||||||
|
self.half_angle_deg = (field.bearing_error_half_deg
|
||||||
|
+ config.search.bearing_rounding_margin_deg)
|
||||||
|
# 覆盖 [-R-m, R+m]² 的格点阵列;格 (i,j) 的中心在
|
||||||
|
# (x0 + (i+0.5)h, x0 + (j+0.5)h),中心到格内任一点 <= h·√2/2 = 裕量
|
||||||
|
span = field.omega_radius_m + self.margin_m
|
||||||
|
self.x0 = -span
|
||||||
|
self.width = int(math.ceil(2 * span / self.spacing_m)) + 1
|
||||||
|
limit_sq = (span + 1e-9) ** 2
|
||||||
|
self.alive: set[int] = set()
|
||||||
|
self._coarse: dict[int, int] = {} # 粗格索引 -> 幸存细格数
|
||||||
|
coarse_w = self._coarse_width()
|
||||||
|
for i in range(self.width):
|
||||||
|
x = self.x0 + (i + 0.5) * self.spacing_m
|
||||||
|
for j in range(self.width):
|
||||||
|
if x * x + (self.x0 + (j + 0.5) * self.spacing_m) ** 2 <= limit_sq:
|
||||||
|
index = i * self.width + j
|
||||||
|
self.alive.add(index)
|
||||||
|
key = (i // _COARSE_FACTOR) * coarse_w + j // _COARSE_FACTOR
|
||||||
|
self._coarse[key] = self._coarse.get(key, 0) + 1
|
||||||
|
self.version = 0
|
||||||
|
self._mec: tuple[Vec2, float] | None = None
|
||||||
|
|
||||||
|
# ------------------------------------------------------------ 索引与坐标
|
||||||
|
def _coarse_width(self) -> int:
|
||||||
|
return (self.width + _COARSE_FACTOR - 1) // _COARSE_FACTOR
|
||||||
|
|
||||||
|
def cell_center(self, index: int) -> Vec2:
|
||||||
|
i, j = divmod(index, self.width)
|
||||||
|
return (self.x0 + (i + 0.5) * self.spacing_m,
|
||||||
|
self.x0 + (j + 0.5) * self.spacing_m)
|
||||||
|
|
||||||
|
def _discard(self, indices: list[int]) -> None:
|
||||||
|
if not indices:
|
||||||
|
return
|
||||||
|
coarse_w = self._coarse_width()
|
||||||
|
for index in indices:
|
||||||
|
if index not in self.alive:
|
||||||
|
continue
|
||||||
|
self.alive.discard(index)
|
||||||
|
key = (index // self.width // _COARSE_FACTOR) * coarse_w \
|
||||||
|
+ (index % self.width) // _COARSE_FACTOR
|
||||||
|
count = self._coarse.get(key, 0) - 1
|
||||||
|
if count <= 0:
|
||||||
|
self._coarse.pop(key, None)
|
||||||
|
else:
|
||||||
|
self._coarse[key] = count
|
||||||
|
self.version += 1
|
||||||
|
self._mec = None
|
||||||
|
|
||||||
|
# ------------------------------------------------------------ 观测更新
|
||||||
|
def _kill_disk(self, center: Vec2, radius_m: float) -> None:
|
||||||
|
"""删除格心距 center <= radius_m 的全部幸存格(整格判据见模块注释)。"""
|
||||||
|
if not self.alive:
|
||||||
|
return
|
||||||
|
reach = radius_m + _DIST_EPS_M
|
||||||
|
ilo = max(0, int(math.floor((center[0] - reach - self.x0) / self.spacing_m)))
|
||||||
|
ihi = min(self.width - 1,
|
||||||
|
int(math.ceil((center[0] + reach - self.x0) / self.spacing_m)))
|
||||||
|
jlo = max(0, int(math.floor((center[1] - reach - self.x0) / self.spacing_m)))
|
||||||
|
jhi = min(self.width - 1,
|
||||||
|
int(math.ceil((center[1] + reach - self.x0) / self.spacing_m)))
|
||||||
|
limit_sq = reach * reach
|
||||||
|
doomed = []
|
||||||
|
for i in range(ilo, ihi + 1):
|
||||||
|
dx = self.x0 + (i + 0.5) * self.spacing_m - center[0]
|
||||||
|
base = i * self.width
|
||||||
|
for j in range(jlo, jhi + 1):
|
||||||
|
index = base + j
|
||||||
|
if index not in self.alive:
|
||||||
|
continue
|
||||||
|
dy = self.x0 + (j + 0.5) * self.spacing_m - center[1]
|
||||||
|
if dx * dx + dy * dy <= limit_sq:
|
||||||
|
doomed.append(index)
|
||||||
|
self._discard(doomed)
|
||||||
|
|
||||||
|
def _kill_outside_disk(self, center: Vec2, radius_m: float) -> None:
|
||||||
|
"""删除格心距 center > radius_m 的全部幸存格(保留盘内)。"""
|
||||||
|
if not self.alive:
|
||||||
|
return
|
||||||
|
reach = radius_m + _DIST_EPS_M
|
||||||
|
doomed = [index for index in self.alive
|
||||||
|
if math.dist(self.cell_center(index), center) > reach]
|
||||||
|
self._discard(doomed)
|
||||||
|
|
||||||
|
def _kill_outside_wedge(self, station: Vec2, reading_deg: float) -> None:
|
||||||
|
"""删除角向偏离读数超出 δ_eff + 角向格裕量的幸存格。
|
||||||
|
|
||||||
|
角向格裕量 = asin(m / max(dist−m, 1)):格内任一点相对格心的方位
|
||||||
|
偏差不超过它(点距测点还可再近 m,故分母再内缩 m)。
|
||||||
|
"""
|
||||||
|
if not self.alive:
|
||||||
|
return
|
||||||
|
sx, sy = float(station[0]), float(station[1])
|
||||||
|
doomed = []
|
||||||
|
for index in self.alive:
|
||||||
|
gx, gy = self.cell_center(index)
|
||||||
|
dist = math.hypot(gx - sx, gy - sy)
|
||||||
|
if dist <= self.margin_m + 1.0:
|
||||||
|
continue # 格贴测点:格内方位可指向任意方向,不可删
|
||||||
|
angular_margin = math.degrees(
|
||||||
|
math.asin(min(0.9999, self.margin_m / (dist - self.margin_m))))
|
||||||
|
gap = angular_gap(math.degrees(math.atan2(gy - sy, gx - sx)), reading_deg)
|
||||||
|
if gap > self.half_angle_deg + angular_margin + _ANG_EPS_DEG:
|
||||||
|
doomed.append(index)
|
||||||
|
self._discard(doomed)
|
||||||
|
|
||||||
|
def apply_measure(self, station: Vec2, result: str,
|
||||||
|
svd_deg: float | None = None) -> None:
|
||||||
|
"""按读数更新幸存集。真源保留性由 Voronoi 裕量保证(模块注释)。"""
|
||||||
|
if result == NO_SIGNAL:
|
||||||
|
# no_signal ⟹ dist > r_eff >= 1000:整格落入 B(s, 1000−m) 的删。
|
||||||
|
self._kill_disk(station, self.r_eff_min_m - self.margin_m)
|
||||||
|
elif result == NEAR:
|
||||||
|
self._kill_outside_disk(station, self.near_radius_m + self.margin_m)
|
||||||
|
elif result == DIRECTION:
|
||||||
|
if svd_deg is None:
|
||||||
|
raise ValueError("direction 读数必须携带 svd_deg")
|
||||||
|
# direction 隐含 dist <= r_max [题面]
|
||||||
|
self._kill_outside_disk(station, self.r_eff_max_m + self.margin_m)
|
||||||
|
self._kill_outside_wedge(station, svd_deg)
|
||||||
|
else:
|
||||||
|
raise ValueError(f"未知测向结果: {result!r}")
|
||||||
|
|
||||||
|
# ------------------------------------------------------------ 查询
|
||||||
|
def __len__(self) -> int:
|
||||||
|
return len(self.alive)
|
||||||
|
|
||||||
|
def alive_points(self, limit: int | None = None) -> list[Vec2]:
|
||||||
|
sorted_alive = sorted(self.alive)
|
||||||
|
if limit is not None and len(sorted_alive) > limit:
|
||||||
|
step = len(sorted_alive) / limit
|
||||||
|
indices = {sorted_alive[int(k * step) % len(sorted_alive)]
|
||||||
|
for k in range(limit)}
|
||||||
|
indices = sorted(indices)
|
||||||
|
else:
|
||||||
|
indices = sorted_alive
|
||||||
|
return [self.cell_center(index) for index in indices]
|
||||||
|
|
||||||
|
def nearest_alive_distance(self, point: Vec2, limit_m: float) -> float | None:
|
||||||
|
"""最近幸存格心距离;> limit_m 时返回 None(粗格环扫 + 下界剪枝)。
|
||||||
|
|
||||||
|
返回值语义:恰为最近幸存格心距离(若 <= limit_m);None 表示所有
|
||||||
|
幸存格心都超出 limit_m。粗格聚合半径已计入扫描范围,不漏判。
|
||||||
|
"""
|
||||||
|
if not self.alive:
|
||||||
|
return None
|
||||||
|
coarse_cell_m = self.spacing_m * _COARSE_FACTOR
|
||||||
|
coarse_reach = coarse_cell_m * math.sqrt(2) / 2
|
||||||
|
cw = self._coarse_width()
|
||||||
|
ci_base = int(math.floor((point[0] - self.x0) / coarse_cell_m))
|
||||||
|
cj_base = int(math.floor((point[1] - self.x0) / coarse_cell_m))
|
||||||
|
ring_max = int(math.ceil((limit_m + coarse_reach) / coarse_cell_m)) + 1
|
||||||
|
best: float | None = None
|
||||||
|
for ring in range(ring_max + 1):
|
||||||
|
lower_bound = max(0.0, ring - 1) * coarse_cell_m - coarse_reach
|
||||||
|
if best is not None and lower_bound >= best:
|
||||||
|
break # 更外环的所有格心都 >= 下界 >= 当前最优
|
||||||
|
for ci in range(max(0, ci_base - ring), min(cw - 1, ci_base + ring) + 1):
|
||||||
|
for cj in range(max(0, cj_base - ring), min(cw - 1, cj_base + ring) + 1):
|
||||||
|
if max(abs(ci - ci_base), abs(cj - cj_base)) != ring:
|
||||||
|
continue # 只扫当前环(Chebyshev 环)
|
||||||
|
if (ci * cw + cj) not in self._coarse:
|
||||||
|
continue
|
||||||
|
ccx = self.x0 + (ci + 0.5) * coarse_cell_m
|
||||||
|
ccy = self.x0 + (cj + 0.5) * coarse_cell_m
|
||||||
|
if math.hypot(ccx - point[0], ccy - point[1]) \
|
||||||
|
> limit_m + coarse_reach:
|
||||||
|
continue
|
||||||
|
i0, j0 = ci * _COARSE_FACTOR, cj * _COARSE_FACTOR
|
||||||
|
for i in range(i0, min(i0 + _COARSE_FACTOR, self.width)):
|
||||||
|
base = i * self.width
|
||||||
|
for j in range(j0, min(j0 + _COARSE_FACTOR, self.width)):
|
||||||
|
index = base + j
|
||||||
|
if index in self.alive:
|
||||||
|
dist = math.dist(self.cell_center(index), point)
|
||||||
|
if dist <= limit_m and (best is None or dist < best):
|
||||||
|
best = dist
|
||||||
|
return best
|
||||||
|
|
||||||
|
def guaranteed_no_signal(self, station: Vec2) -> bool:
|
||||||
|
"""真源与该测点距离必 > r_max(>= r_eff)⟹ 必 no_signal,测了也无信息。"""
|
||||||
|
return self.nearest_alive_distance(
|
||||||
|
station, self.r_eff_max_m + self.margin_m) is None
|
||||||
|
|
||||||
|
def mec(self) -> tuple[Vec2, float]:
|
||||||
|
"""幸存集最小包围圆(圆心, 半径);按 version 缓存。"""
|
||||||
|
if self._mec is None:
|
||||||
|
if not self.alive:
|
||||||
|
raise ValueError("幸存集为空(该频道已可证明缺席)")
|
||||||
|
points = [self.cell_center(index) for index in self.alive]
|
||||||
|
self._mec = smallest_enclosing_circle(points)
|
||||||
|
return self._mec
|
||||||
|
|
||||||
|
# ------------------------------------------------------------ 信息增益
|
||||||
|
def subsample(self, size: int) -> list[Vec2]:
|
||||||
|
"""按索引序均匀抽取的子采样(索引序≈空间序,代表点空间铺开)。"""
|
||||||
|
return self.alive_points(limit=size)
|
||||||
|
|
||||||
|
def expected_survival(self, point: Vec2, subsample: list[Vec2],
|
||||||
|
representatives: list[Vec2]) -> float:
|
||||||
|
"""E[log 幸存占比]:越小该测点的期望信息增益越大。
|
||||||
|
|
||||||
|
r_eff 不确定性解析积分:dist∈(1000,1500] 时按 r_eff~U(1000,1500)
|
||||||
|
取 p_ns=(dist−1000)/500 为 no_signal 分支概率。
|
||||||
|
- no_signal 分支:删 B(point, 1000−m),幸存 = 子采样中盘外比例;
|
||||||
|
- direction 分支:以代表点方位为期望读数,角域 ∩ B(point,1500+m) 内幸存;
|
||||||
|
- near 分支(dist <= 5+m):可行集收缩到 5m 盘,幸存占比趋近 0。
|
||||||
|
"""
|
||||||
|
if not self.alive or not subsample:
|
||||||
|
return 0.0
|
||||||
|
eps = 1.0 / len(subsample)
|
||||||
|
keep_ns = max(1.0 - self.fraction_within(subsample, point,
|
||||||
|
self.r_eff_min_m - self.margin_m),
|
||||||
|
eps)
|
||||||
|
log_keep_ns = math.log(keep_ns)
|
||||||
|
total = 0.0
|
||||||
|
for rep in representatives:
|
||||||
|
dist = math.hypot(rep[0] - point[0], rep[1] - point[1])
|
||||||
|
if dist <= self.near_radius_m + self.margin_m:
|
||||||
|
total += math.log(eps)
|
||||||
|
continue
|
||||||
|
p_ns = min(1.0, max(0.0, (dist - self.r_eff_min_m)
|
||||||
|
/ (self.r_eff_max_m - self.r_eff_min_m)))
|
||||||
|
reading = math.atan2(rep[1] - point[1], rep[0] - point[0])
|
||||||
|
survivors = 0
|
||||||
|
for p in subsample:
|
||||||
|
if math.dist(p, point) > self.r_eff_max_m + self.margin_m:
|
||||||
|
continue
|
||||||
|
gap = angular_gap(math.degrees(
|
||||||
|
math.atan2(p[1] - point[1], p[0] - point[0])),
|
||||||
|
math.degrees(reading))
|
||||||
|
if gap <= self.half_angle_deg + _ANG_EPS_DEG:
|
||||||
|
survivors += 1
|
||||||
|
keep_dir = max(survivors / len(subsample), eps)
|
||||||
|
total += p_ns * log_keep_ns + (1.0 - p_ns) * math.log(keep_dir)
|
||||||
|
return total / max(len(representatives), 1)
|
||||||
|
|
||||||
|
def fraction_within(self, samples: list[Vec2], point: Vec2,
|
||||||
|
radius_m: float) -> float:
|
||||||
|
if not samples:
|
||||||
|
return 0.0
|
||||||
|
reach = radius_m + _DIST_EPS_M
|
||||||
|
count = sum(1 for p in samples if math.dist(p, point) <= reach)
|
||||||
|
return count / len(samples)
|
||||||
|
|
||||||
|
def _branch_radius(self, members: list[Vec2]) -> float:
|
||||||
|
"""分支后验的半径代理:幸存子采样到其质心的最大距离(O(n),免 Welzl)。"""
|
||||||
|
if not members:
|
||||||
|
return 0.0
|
||||||
|
mx = sum(p[0] for p in members) / len(members)
|
||||||
|
my = sum(p[1] for p in members) / len(members)
|
||||||
|
return max(math.hypot(p[0] - mx, p[1] - my) for p in members)
|
||||||
|
|
||||||
|
def expected_posterior_radius(self, point: Vec2, subsample: list[Vec2],
|
||||||
|
representatives: list[Vec2]) -> float:
|
||||||
|
"""一次测量后期望后验半径(米):选点主指标,直指清除证书阈值。
|
||||||
|
|
||||||
|
对每个代表假设(真值候选)按 r_eff~U(1000,1500) 解析积分
|
||||||
|
no_signal/direction 分支,near 分支(dist <= 5+m)后验为 5m 盘;
|
||||||
|
各分支半径用幸存子采样的质心最大距离近似(偏差 << 格距,排序用)。
|
||||||
|
与几何版 worst-posterior 同族,但作用于含负观测的非凸幸存集,
|
||||||
|
且以期望代替最坏(代表点覆盖幸存集主轴)。
|
||||||
|
"""
|
||||||
|
if not self.alive or not subsample:
|
||||||
|
return 0.0
|
||||||
|
eps_radius = self.spacing_m / 2.0
|
||||||
|
total = 0.0
|
||||||
|
for rep in representatives:
|
||||||
|
dist = math.hypot(rep[0] - point[0], rep[1] - point[1])
|
||||||
|
if dist <= self.near_radius_m + self.margin_m:
|
||||||
|
total += self.near_radius_m + self.margin_m
|
||||||
|
continue
|
||||||
|
p_ns = min(1.0, max(0.0, (dist - self.r_eff_min_m)
|
||||||
|
/ (self.r_eff_max_m - self.r_eff_min_m)))
|
||||||
|
members_ns = [p for p in subsample
|
||||||
|
if math.dist(p, point) > self.r_eff_min_m - self.margin_m]
|
||||||
|
reading = math.atan2(rep[1] - point[1], rep[0] - point[0])
|
||||||
|
members_dir = []
|
||||||
|
for p in subsample:
|
||||||
|
if math.dist(p, point) > self.r_eff_max_m + self.margin_m:
|
||||||
|
continue
|
||||||
|
gap = angular_gap(math.degrees(
|
||||||
|
math.atan2(p[1] - point[1], p[0] - point[0])),
|
||||||
|
math.degrees(reading))
|
||||||
|
if gap <= self.half_angle_deg + _ANG_EPS_DEG:
|
||||||
|
members_dir.append(p)
|
||||||
|
r_ns = self._branch_radius(members_ns) if members_ns else eps_radius
|
||||||
|
r_dir = self._branch_radius(members_dir) if members_dir else eps_radius
|
||||||
|
total += p_ns * r_ns + (1.0 - p_ns) * r_dir
|
||||||
|
return total / max(len(representatives), 1)
|
||||||
@@ -0,0 +1,429 @@
|
|||||||
|
"""P4b 贝叶斯网格滤波闭环(问题3新策略 ``bayes``)。
|
||||||
|
|
||||||
|
在 :class:`OmnidirectionalLoop` 骨架(run/异常收尾/summary/预算/异常留痕)
|
||||||
|
上的三层重构,对应 docs/algo-3-bayes.md 的架构:
|
||||||
|
|
||||||
|
1. 感知层:每频道一个网格幸存集(particles.ChannelParticles),全部读数
|
||||||
|
(含 no_signal 负观测)即时更新;幸存集为空即缺席证明,无需等覆盖账本;
|
||||||
|
2. 决策层:统一任务循环——站点扫频、定位测量、清除三类任务合成任务表,
|
||||||
|
开放路径最近邻 + 2-opt 排序,每执行一个任务滚动重排(动态 TSP);
|
||||||
|
3. 选点层:定位测量点在保证接收盘(凸证书)或 PF MEC 探索环(宽候选集)
|
||||||
|
内按"期望 log 幸存占比 + λ·移动时间"评分(信息增益贪心)。
|
||||||
|
|
||||||
|
与基线 SevenBatch 的差异:负观测跳站/跳测(必 no_signal 的 (站,频道) 对
|
||||||
|
不消耗检测与换频)、PF 缺席证明提前证毕、全局任务排序替代"站内最近邻"。
|
||||||
|
清除证书仍以凸证书为主(PF 网格证书仅在负观测挖洞使幸存集更紧时提前
|
||||||
|
触发),两者均满足 Voronoi/保守裕量,不牺牲全清率。
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import math
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
from ..belief import ChannelStatus
|
||||||
|
from ..config import ProblemConfig, Vec2
|
||||||
|
from ..coverage import CoverageLedger
|
||||||
|
from ..particles import ChannelParticles
|
||||||
|
from ..protocol.client import SimulatorClient
|
||||||
|
from ..protocol.rules import DIRECTION, NEAR, NO_SIGNAL
|
||||||
|
from ..protocol.timing import move_time
|
||||||
|
from . import (ClearCertificate, OmnidirectionalLoop, clear_certificate,
|
||||||
|
candidate_points, safe_disk_radius, select_measurement_point)
|
||||||
|
from .seven import seven_points
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class Task:
|
||||||
|
"""统一任务表的一项。service_s 为不含移动的固有耗时(检测+换频/清除)。"""
|
||||||
|
|
||||||
|
kind: str # "station" | "locate" | "clear"
|
||||||
|
position: Vec2
|
||||||
|
service_s: float
|
||||||
|
channel: int | None = None
|
||||||
|
station_index: int | None = None
|
||||||
|
|
||||||
|
|
||||||
|
def _point_key(point: Vec2) -> tuple[int, int]:
|
||||||
|
"""候选点去重键:0.5m 网格取整(同一物理点不重复测量)。"""
|
||||||
|
return (int(round(point[0] * 2)), int(round(point[1] * 2)))
|
||||||
|
|
||||||
|
|
||||||
|
class BayesLoop(OmnidirectionalLoop):
|
||||||
|
"""问题3贝叶斯闭环:负观测感知 + 信息增益选点 + 滚动任务排序。"""
|
||||||
|
|
||||||
|
def __init__(self, client: SimulatorClient,
|
||||||
|
config: ProblemConfig = ProblemConfig()):
|
||||||
|
super().__init__(client, config)
|
||||||
|
self.ledger = CoverageLedger(
|
||||||
|
config, points=seven_points(config.bayes.station_ring_radius_m))
|
||||||
|
self.particles = {channel: ChannelParticles(config)
|
||||||
|
for channel in self.belief.channels}
|
||||||
|
self._measured_points: dict[int, set[tuple[int, int]]] = {}
|
||||||
|
self._clear_failed: set[int] = set() # 证书清除失败后须先补测再重试
|
||||||
|
self._stall: dict[int, int] = {} # 连续无收缩计数(异常留痕用)
|
||||||
|
self._skip_cache: dict[tuple[int, int], tuple[int, bool]] = {}
|
||||||
|
self.skipped_measures = 0 # 跳测规则省下的检测次数
|
||||||
|
self.no_shrink_measures = 0 # 未收缩幸存集的定位检测(诊断用)
|
||||||
|
self.pf_absent_channels: list[int] = [] # 由幸存集为空证明缺席的频道
|
||||||
|
|
||||||
|
# ------------------------------------------------------------ 主循环
|
||||||
|
def _coverage_phase(self) -> None:
|
||||||
|
"""统一任务循环:发现 + 定位 + 清除滚动调度,直到任务表为空。"""
|
||||||
|
while self.stop_reason is None:
|
||||||
|
if self._count_limit_reached() or self._virtual_cap_reached():
|
||||||
|
return
|
||||||
|
if not self._step_once():
|
||||||
|
return
|
||||||
|
|
||||||
|
def _step_once(self) -> bool:
|
||||||
|
"""构建任务表、排序并执行首任务;无可执行任务返回 False。"""
|
||||||
|
tasks = self._build_tasks()
|
||||||
|
if not tasks:
|
||||||
|
return False
|
||||||
|
order = self._order_tasks(tasks)
|
||||||
|
self._execute_task(order[0])
|
||||||
|
return True
|
||||||
|
|
||||||
|
# ------------------------------------------------------------ 任务构建
|
||||||
|
def _station_pending(self, index: int) -> list[int]:
|
||||||
|
"""站点待测频道:UNKNOWN、未测过、非"必 no_signal"且未证缺席。"""
|
||||||
|
point = self.ledger.points[index]
|
||||||
|
pending = []
|
||||||
|
for channel, belief in self.belief.channels.items():
|
||||||
|
if belief.status is not ChannelStatus.UNKNOWN:
|
||||||
|
continue
|
||||||
|
if self.ledger.measured_at(channel, index):
|
||||||
|
continue
|
||||||
|
if self._skip_station_channel(channel, index, point):
|
||||||
|
continue
|
||||||
|
pending.append(channel)
|
||||||
|
return pending
|
||||||
|
|
||||||
|
def _skip_station_channel(self, channel: int, index: int, point: Vec2) -> bool:
|
||||||
|
"""跳测规则(按 pf.version 缓存;幸存集单调收缩,判定单调)。"""
|
||||||
|
pf = self.particles[channel]
|
||||||
|
if not len(pf):
|
||||||
|
return True
|
||||||
|
cached = self._skip_cache.get((channel, index))
|
||||||
|
if cached is not None and cached[0] == pf.version:
|
||||||
|
return cached[1]
|
||||||
|
value = pf.guaranteed_no_signal(point)
|
||||||
|
self._skip_cache[(channel, index)] = (pf.version, value)
|
||||||
|
return value
|
||||||
|
|
||||||
|
def _build_tasks(self) -> list[Task]:
|
||||||
|
tasks: list[Task] = []
|
||||||
|
for index, point in enumerate(self.ledger.points):
|
||||||
|
pending = self._station_pending(index)
|
||||||
|
if not pending:
|
||||||
|
continue
|
||||||
|
switches = len(pending) - (1 if self.client.state.channel in pending else 0)
|
||||||
|
tasks.append(Task("station", tuple(point),
|
||||||
|
self.config.cost.measure_s * len(pending)
|
||||||
|
+ self.config.cost.switch_s * max(switches, 0),
|
||||||
|
station_index=index))
|
||||||
|
for channel in self.belief.channels_by_status(ChannelStatus.DETECTED):
|
||||||
|
cert = self._best_certificate(channel)
|
||||||
|
if cert is not None and cert.certified \
|
||||||
|
and channel not in self._clear_failed:
|
||||||
|
tasks.append(Task("clear", cert.center,
|
||||||
|
self.config.cost.clear_success_s,
|
||||||
|
channel=channel))
|
||||||
|
continue
|
||||||
|
point = self._select_point(channel)
|
||||||
|
if point is None:
|
||||||
|
continue
|
||||||
|
tasks.append(Task("locate", point,
|
||||||
|
self.config.cost.measure_s + self.config.cost.switch_s,
|
||||||
|
channel=channel))
|
||||||
|
return tasks
|
||||||
|
|
||||||
|
# ------------------------------------------------------------ 任务排序
|
||||||
|
def _order_tasks(self, tasks: list[Task]) -> list[Task]:
|
||||||
|
"""开放路径排序:最近邻打底,2-opt 改进(终点自由,不回起点)。"""
|
||||||
|
if len(tasks) <= 2:
|
||||||
|
return sorted(tasks, key=lambda t: (
|
||||||
|
move_time(self.client.state.position, t.position, self.config.cost),
|
||||||
|
t.kind, t.channel or 0))
|
||||||
|
order = self._nearest_order(tasks)
|
||||||
|
if not self.config.bayes.two_opt_enabled:
|
||||||
|
return order
|
||||||
|
return self._two_opt(order)
|
||||||
|
|
||||||
|
def _nearest_order(self, tasks: list[Task]) -> list[Task]:
|
||||||
|
remaining = list(tasks)
|
||||||
|
order = []
|
||||||
|
position = self.client.state.position
|
||||||
|
while remaining:
|
||||||
|
best = min(remaining, key=lambda t: (
|
||||||
|
move_time(position, t.position, self.config.cost),
|
||||||
|
t.kind, t.channel or 0))
|
||||||
|
remaining.remove(best)
|
||||||
|
order.append(best)
|
||||||
|
position = best.position
|
||||||
|
return order
|
||||||
|
|
||||||
|
def _path_cost(self, order: list[Task]) -> float:
|
||||||
|
total = 0.0
|
||||||
|
position = self.client.state.position
|
||||||
|
for task in order:
|
||||||
|
total += move_time(position, task.position, self.config.cost)
|
||||||
|
total += task.service_s
|
||||||
|
position = task.position
|
||||||
|
return total
|
||||||
|
|
||||||
|
def _two_opt(self, order: list[Task]) -> list[Task]:
|
||||||
|
best = list(order)
|
||||||
|
best_cost = self._path_cost(best)
|
||||||
|
for _ in range(self.config.bayes.two_opt_passes):
|
||||||
|
improved = False
|
||||||
|
for i in range(len(best) - 1):
|
||||||
|
for j in range(i + 1, len(best)):
|
||||||
|
candidate = best[:i] + best[i:j + 1][::-1] + best[j + 1:]
|
||||||
|
cost = self._path_cost(candidate)
|
||||||
|
if cost < best_cost - 1e-6:
|
||||||
|
best, best_cost, improved = candidate, cost, True
|
||||||
|
if not improved:
|
||||||
|
break
|
||||||
|
return best
|
||||||
|
|
||||||
|
# ------------------------------------------------------------ 任务执行
|
||||||
|
def _execute_task(self, task: Task) -> None:
|
||||||
|
if task.kind == "station":
|
||||||
|
self._execute_station(task.station_index)
|
||||||
|
elif task.kind == "clear":
|
||||||
|
cert = self._best_certificate(task.channel)
|
||||||
|
if cert is not None and cert.certified:
|
||||||
|
if not self._attempt_clear(task.channel, cert.center, cert):
|
||||||
|
self._clear_failed.add(task.channel)
|
||||||
|
self._may_clear.discard(task.channel)
|
||||||
|
else:
|
||||||
|
self._clear_failed.discard(task.channel)
|
||||||
|
elif task.kind == "locate":
|
||||||
|
self._execute_locate(task.channel, task.position)
|
||||||
|
|
||||||
|
def _execute_station(self, index: int) -> None:
|
||||||
|
"""单站批量扫频:只测待测频道(跳测规则已过滤),扫完即返回重排。"""
|
||||||
|
point = self.ledger.points[index]
|
||||||
|
pending = self._station_pending(index)
|
||||||
|
unknown = [c for c, b in self.belief.channels.items()
|
||||||
|
if b.status is ChannelStatus.UNKNOWN]
|
||||||
|
self.skipped_measures += len(unknown) - len(pending)
|
||||||
|
# 当前频道优先,减少换频;同站 near 直接清除不打断扫频
|
||||||
|
pending.sort(key=lambda c: (c != self.client.state.channel, c))
|
||||||
|
for channel in pending:
|
||||||
|
if self.stop_reason is not None or self._count_limit_reached() \
|
||||||
|
or self._virtual_cap_reached():
|
||||||
|
return
|
||||||
|
outcome = self.client.measure(point, channel)
|
||||||
|
self.ledger.mark_measured(channel, index)
|
||||||
|
self._record_measure(channel, point, outcome)
|
||||||
|
if outcome.result in (DIRECTION, NEAR):
|
||||||
|
self._may_clear.add(channel)
|
||||||
|
if outcome.result == NEAR:
|
||||||
|
region = self.belief.candidate_set(channel)
|
||||||
|
cert = clear_certificate(region, channel, self.config)
|
||||||
|
if cert.certified:
|
||||||
|
if self._attempt_clear(channel, point, cert):
|
||||||
|
self._clear_failed.discard(channel)
|
||||||
|
else:
|
||||||
|
self._clear_failed.add(channel)
|
||||||
|
self._may_clear.discard(channel)
|
||||||
|
else:
|
||||||
|
self._anomaly("near_without_certificate", channel, position=point)
|
||||||
|
|
||||||
|
def _execute_locate(self, channel: int, point: Vec2) -> None:
|
||||||
|
"""单步定位测量:执行后立即重排(滚动时域),收敛则顺路清除。"""
|
||||||
|
pf = self.particles[channel]
|
||||||
|
before = len(pf)
|
||||||
|
outcome = self.client.measure(point, channel)
|
||||||
|
self._record_measure(channel, point, outcome)
|
||||||
|
self._may_clear.add(channel)
|
||||||
|
if outcome.result == NO_SIGNAL:
|
||||||
|
region = self.belief.candidate_set(channel)
|
||||||
|
if region is not None and safe_disk_radius(region, self.config) >= 0 \
|
||||||
|
and math.dist(point, region.mec()[0]) \
|
||||||
|
<= safe_disk_radius(region, self.config):
|
||||||
|
self._anomaly("no_signal_in_safe_disk", channel, position=point)
|
||||||
|
after = len(pf)
|
||||||
|
cert = self._best_certificate(channel)
|
||||||
|
if cert is not None and cert.certified:
|
||||||
|
if self._attempt_clear(channel, cert.center, cert):
|
||||||
|
self._clear_failed.discard(channel)
|
||||||
|
self._stall.pop(channel, None)
|
||||||
|
return
|
||||||
|
self._clear_failed.add(channel)
|
||||||
|
self._may_clear.discard(channel)
|
||||||
|
if after == before:
|
||||||
|
self._stall[channel] = self._stall.get(channel, 0) + 1
|
||||||
|
self.no_shrink_measures += 1 # 只是浪费不破坏守恒性,不进异常表
|
||||||
|
else:
|
||||||
|
self._stall[channel] = 0
|
||||||
|
|
||||||
|
# ------------------------------------------------------------ 感知与证书
|
||||||
|
def _record_measure(self, channel: int, position: Vec2, outcome) -> None:
|
||||||
|
"""belief(凸证书)与 PF(幸存集)同步更新;PF 空即证缺席。"""
|
||||||
|
self.belief.update_measure(channel, position, outcome.result,
|
||||||
|
outcome.svd_deg)
|
||||||
|
self._measured_points.setdefault(channel, set()).add(_point_key(position))
|
||||||
|
self._clear_failed.discard(channel) # 新证据落地:允许再次尝试清除
|
||||||
|
belief = self.belief.channels[channel]
|
||||||
|
if belief.status in (ChannelStatus.CLEARED, ChannelStatus.ABSENT):
|
||||||
|
return # 已证毕:清除/缺席后的 no_signal 与距离无关,不进 PF
|
||||||
|
pf = self.particles[channel]
|
||||||
|
pf.apply_measure(position, outcome.result, outcome.svd_deg)
|
||||||
|
if not len(pf):
|
||||||
|
if belief.status is ChannelStatus.UNKNOWN:
|
||||||
|
self.belief.mark_absent(channel)
|
||||||
|
if channel not in self.pf_absent_channels:
|
||||||
|
self.pf_absent_channels.append(channel)
|
||||||
|
else:
|
||||||
|
self._anomaly("pf_empty_while_detected", channel, position=position)
|
||||||
|
|
||||||
|
def _best_certificate(self, channel: int) -> ClearCertificate | None:
|
||||||
|
"""凸证书优先;负观测挖洞使 PF 幸存集更紧时用网格证书补位。"""
|
||||||
|
region = self.belief.candidate_set(channel)
|
||||||
|
cert = clear_certificate(region, channel, self.config) \
|
||||||
|
if region is not None else None
|
||||||
|
if cert is not None and cert.certified:
|
||||||
|
return cert
|
||||||
|
if self.config.bayes.grid_cert_enabled:
|
||||||
|
pf = self.particles[channel]
|
||||||
|
if len(pf):
|
||||||
|
center, radius = pf.mec()
|
||||||
|
limit = (self.config.field.clear_radius_m - pf.margin_m
|
||||||
|
- self.config.search.clear_margin_m)
|
||||||
|
if radius <= limit:
|
||||||
|
return ClearCertificate(channel, center, radius, limit, True)
|
||||||
|
return cert
|
||||||
|
|
||||||
|
# ------------------------------------------------------------ 信息增益选点
|
||||||
|
def _select_point(self, channel: int) -> Vec2 | None:
|
||||||
|
"""定位测量点:期望 log 幸存占比 + λ·移动时间 最小的候选。"""
|
||||||
|
bayes, search = self.config.bayes, self.config.search
|
||||||
|
pf = self.particles[channel]
|
||||||
|
if not len(pf):
|
||||||
|
return None
|
||||||
|
region = self.belief.candidate_set(channel)
|
||||||
|
center, radius = pf.mec()
|
||||||
|
measured = self._measured_points.get(channel, set())
|
||||||
|
candidates: list[Vec2] = []
|
||||||
|
safe = safe_disk_radius(region, self.config) if region is not None else -1.0
|
||||||
|
if region is not None and safe >= 0:
|
||||||
|
# 保证接收盘(凸证书口径):任意候选必产生 direction/near
|
||||||
|
candidates = candidate_points(tuple(region.mec()[0]), safe,
|
||||||
|
search.candidate_rings,
|
||||||
|
search.candidate_angles)
|
||||||
|
else:
|
||||||
|
# 宽候选集:PF MEC 探索环(负观测挖洞后幸存集的包围圆)
|
||||||
|
for fraction in bayes.wide_rings:
|
||||||
|
ring = max(radius * fraction, 2.0 * pf.margin_m)
|
||||||
|
candidates.extend(candidate_points(center, ring, (1.0,),
|
||||||
|
search.candidate_angles))
|
||||||
|
candidates.append(tuple(center))
|
||||||
|
candidates = [c for c in candidates if _point_key(c) not in measured]
|
||||||
|
if not candidates:
|
||||||
|
return None
|
||||||
|
subsample = pf.subsample(bayes.score_subsample)
|
||||||
|
stride = max(len(subsample) // bayes.mc_representatives, 1)
|
||||||
|
representatives = [subsample[k]
|
||||||
|
for k in range(0, len(subsample), stride)]
|
||||||
|
position = self.client.state.position
|
||||||
|
|
||||||
|
def score(q: Vec2) -> float:
|
||||||
|
move = bayes.lambda_move_s * move_time(position, q, self.config.cost)
|
||||||
|
if bayes.score_mode == "entropy":
|
||||||
|
return (pf.expected_survival(q, subsample, representatives)
|
||||||
|
+ move)
|
||||||
|
return (pf.expected_posterior_radius(q, subsample, representatives)
|
||||||
|
+ move)
|
||||||
|
|
||||||
|
return min(candidates, key=lambda q: (score(q), _point_key(q)))
|
||||||
|
|
||||||
|
# ------------------------------------------------------------ 安全网与复扫
|
||||||
|
def _localize_and_clear(self, channel: int) -> bool:
|
||||||
|
"""阻塞式定位子循环(resolve 阶段安全网):PF 选点,失败退凸选点。"""
|
||||||
|
search = self.config.search
|
||||||
|
self._may_clear.add(channel)
|
||||||
|
for _ in range(search.max_localize_steps):
|
||||||
|
if self._virtual_cap_reached():
|
||||||
|
return False
|
||||||
|
cert = self._best_certificate(channel)
|
||||||
|
if cert is not None and cert.certified and channel in self._may_clear:
|
||||||
|
if self._attempt_clear(channel, cert.center, cert):
|
||||||
|
self._clear_failed.discard(channel)
|
||||||
|
return True
|
||||||
|
self._clear_failed.add(channel)
|
||||||
|
self._may_clear.discard(channel)
|
||||||
|
continue
|
||||||
|
point = self._select_point(channel)
|
||||||
|
if point is None:
|
||||||
|
region = self.belief.candidate_set(channel)
|
||||||
|
if region is None:
|
||||||
|
return False
|
||||||
|
point = select_measurement_point(
|
||||||
|
region, self.client.state.position, self.config).position
|
||||||
|
outcome = self.client.measure(point, channel)
|
||||||
|
self._record_measure(channel, point, outcome)
|
||||||
|
self._may_clear.add(channel)
|
||||||
|
if outcome.result == NO_SIGNAL:
|
||||||
|
region = self.belief.candidate_set(channel)
|
||||||
|
if region is not None and safe_disk_radius(region, self.config) >= 0 \
|
||||||
|
and math.dist(point, region.mec()[0]) \
|
||||||
|
<= safe_disk_radius(region, self.config):
|
||||||
|
self._anomaly("no_signal_in_safe_disk", channel, position=point)
|
||||||
|
cert = self._best_certificate(channel)
|
||||||
|
if cert is not None and cert.certified:
|
||||||
|
if self._attempt_clear(channel, cert.center, cert):
|
||||||
|
self._clear_failed.discard(channel)
|
||||||
|
return True
|
||||||
|
self._clear_failed.add(channel)
|
||||||
|
self._may_clear.discard(channel)
|
||||||
|
return self.belief.channels[channel].status is ChannelStatus.CLEARED
|
||||||
|
|
||||||
|
def _persistent_round(self) -> None:
|
||||||
|
"""一轮复扫:与基线一致的全站复测,但读数同步进 PF(防假设偏差)。"""
|
||||||
|
measured: set[tuple[int, int]] = set()
|
||||||
|
while self.stop_reason is None:
|
||||||
|
index = self._next_resweep_point(measured)
|
||||||
|
if index is None:
|
||||||
|
return
|
||||||
|
point = self.ledger.points[index]
|
||||||
|
for channel in self._resweep_channels_at(index, measured):
|
||||||
|
if self._virtual_cap_reached():
|
||||||
|
return
|
||||||
|
measured.add((channel, index))
|
||||||
|
was_absent = (self.belief.channels[channel].status
|
||||||
|
is ChannelStatus.ABSENT)
|
||||||
|
outcome = self.client.measure(point, channel)
|
||||||
|
self._record_measure(channel, point, outcome)
|
||||||
|
if was_absent and outcome.result in (DIRECTION, NEAR):
|
||||||
|
# 复测推翻了缺席证明:守恒性留痕,按新证据定位清除。
|
||||||
|
self._anomaly("absent_detected_on_resweep", channel,
|
||||||
|
position=point, result=outcome.result)
|
||||||
|
if outcome.result == DIRECTION:
|
||||||
|
self._localize_and_clear(channel)
|
||||||
|
elif outcome.result == NEAR:
|
||||||
|
cert = self._best_certificate(channel)
|
||||||
|
if cert is not None and cert.certified:
|
||||||
|
if not self._attempt_clear(channel, cert.center, cert):
|
||||||
|
self._clear_failed.add(channel)
|
||||||
|
else:
|
||||||
|
self._anomaly("near_without_certificate", channel,
|
||||||
|
position=point)
|
||||||
|
if self.stop_reason is not None:
|
||||||
|
return
|
||||||
|
|
||||||
|
# ------------------------------------------------------------ 收尾
|
||||||
|
def _summary(self, info) -> dict:
|
||||||
|
report = super()._summary(info)
|
||||||
|
report["strategy"] = "bayes"
|
||||||
|
report["skipped_measures"] = self.skipped_measures
|
||||||
|
report["no_shrink_measures"] = self.no_shrink_measures
|
||||||
|
report["pf_absent_channels"] = sorted(self.pf_absent_channels)
|
||||||
|
report["pf_alive_final"] = {
|
||||||
|
str(channel): len(self.particles[channel])
|
||||||
|
for channel in sorted(self.particles)
|
||||||
|
if self.belief.channels[channel].status is ChannelStatus.DETECTED}
|
||||||
|
return report
|
||||||
@@ -0,0 +1,93 @@
|
|||||||
|
"""P4 七点覆盖策略族:解析证明的六边形发现站 + 同站批量扫频。
|
||||||
|
|
||||||
|
从 scripts/benchmark_q3_search.py 提升为库代码(A2 基线,官方演练
|
||||||
|
12/12 全清、虚拟 5232s)。覆盖几何:原点 + 半径 RING_RADIUS_M 环上
|
||||||
|
6 等分点;切换最近站的半径 a/sqrt(3),域内任意点到最近站距离的解析
|
||||||
|
上界 max(R/sqrt(3), sqrt(Rom³/(sqrt(3)·Rring)+...)) < 1000m [题面
|
||||||
|
r_eff 下界],故任意合法源必被至少一站检出——缺席证明只需该频道
|
||||||
|
7 站全 no_signal(CoverageLedger.coverage_complete)。
|
||||||
|
|
||||||
|
调度变体:
|
||||||
|
- SevenImmediate:站内即时进入定位子循环(A1);
|
||||||
|
- SevenBatch:站内只扫频,扫完按"距候选集 MEC 圆心最近"服务队列(A2);
|
||||||
|
- SevenSweep:全域扫完再统一服务(A3)。
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import math
|
||||||
|
|
||||||
|
from ..belief import ChannelStatus
|
||||||
|
from ..config import ProblemConfig
|
||||||
|
from ..coverage import CoverageLedger
|
||||||
|
from ..protocol.rules import NEAR
|
||||||
|
from . import OmnidirectionalLoop, clear_certificate
|
||||||
|
|
||||||
|
RING_RADIUS_M = 1200.0 # [工程] 解析证明见 docs/algo-3.md §5 与 geometry_check
|
||||||
|
|
||||||
|
|
||||||
|
def seven_points(ring_radius_m: float = RING_RADIUS_M) -> list[tuple[float, float]]:
|
||||||
|
"""原点 + 环上 6 等分点的七站覆盖集。"""
|
||||||
|
return [(0.0, 0.0)] + [(ring_radius_m * math.cos(k * math.pi / 3),
|
||||||
|
ring_radius_m * math.sin(k * math.pi / 3)) for k in range(6)]
|
||||||
|
|
||||||
|
|
||||||
|
class SevenImmediate(OmnidirectionalLoop):
|
||||||
|
"""七点覆盖 + 站内即时定位(基类覆盖阶段逻辑不变,仅换站集)。"""
|
||||||
|
|
||||||
|
def __init__(self, client, config: ProblemConfig = ProblemConfig(),
|
||||||
|
ring_radius_m: float = RING_RADIUS_M):
|
||||||
|
super().__init__(client, config)
|
||||||
|
self.ledger = CoverageLedger(config=config, points=seven_points(ring_radius_m))
|
||||||
|
|
||||||
|
|
||||||
|
class SevenBatch(SevenImmediate):
|
||||||
|
"""站内只扫频,扫完按最近候选集圆心顺序服务检测队列(A2 主策略)。"""
|
||||||
|
|
||||||
|
defer_all = False
|
||||||
|
|
||||||
|
def _serve_queue(self):
|
||||||
|
# Each detected channel gets at most one localization call in this pass.
|
||||||
|
queue = self.belief.channels_by_status(ChannelStatus.DETECTED)
|
||||||
|
while queue:
|
||||||
|
c = min(queue, key=lambda c: (math.dist(self.client.state.position,
|
||||||
|
self.belief.candidate_set(c).mec()[0]), c))
|
||||||
|
queue.remove(c)
|
||||||
|
self._localize_and_clear(c)
|
||||||
|
if self._count_limit_reached():
|
||||||
|
break
|
||||||
|
|
||||||
|
def _coverage_phase(self):
|
||||||
|
while not self._count_limit_reached():
|
||||||
|
index = self._next_point_index()
|
||||||
|
if index is None:
|
||||||
|
return
|
||||||
|
point = self.ledger.points[index]
|
||||||
|
pending = self._pending_channels_at(index)
|
||||||
|
# Keeping the current measuring channel first saves a switch when possible.
|
||||||
|
pending.sort(key=lambda c: (c != self.client.state.channel, c))
|
||||||
|
for c in pending:
|
||||||
|
outcome = self.client.measure(point, c)
|
||||||
|
self.ledger.mark_measured(c, index)
|
||||||
|
self.belief.update_measure(c, point, outcome.result, outcome.svd_deg)
|
||||||
|
if outcome.result == NEAR:
|
||||||
|
cert = clear_certificate(self.belief.candidate_set(c), c, self.config)
|
||||||
|
# Same-station near clear; do not displace the station scan.
|
||||||
|
if cert.certified:
|
||||||
|
self._attempt_clear(c, point, cert)
|
||||||
|
if self._count_limit_reached():
|
||||||
|
return
|
||||||
|
if not self.defer_all:
|
||||||
|
self._serve_queue()
|
||||||
|
|
||||||
|
def _resolve_detected(self):
|
||||||
|
self._serve_queue()
|
||||||
|
unresolved = [c for c, b in self.belief.channels.items()
|
||||||
|
if b.status not in (ChannelStatus.CLEARED, ChannelStatus.ABSENT)]
|
||||||
|
return "all_resolved" if not unresolved else "unresolved_channels"
|
||||||
|
|
||||||
|
|
||||||
|
class SevenSweep(SevenBatch):
|
||||||
|
"""Finish discovery at all necessary stations before servicing the queue."""
|
||||||
|
|
||||||
|
defer_all = True
|
||||||
@@ -0,0 +1,154 @@
|
|||||||
|
"""P4b 贝叶斯闭环端到端验收:全清、PF 缺席证明、预算与故障。
|
||||||
|
|
||||||
|
对齐 test_loop_q3.py 的验收口径(docs/algo-3-bayes.md):
|
||||||
|
- 10/16 源场景全部清除且守恒性零异常(凸证书 + 网格证书均不得失效);
|
||||||
|
- PF 缺席证明(幸存集为空)与覆盖账本口径一致,不误判有源频道;
|
||||||
|
- 无源场景全部频道证明缺席后正常退出(persistent 关闭时 all_resolved);
|
||||||
|
- 近圆域边界源(r_eff=1000)不漏搜;
|
||||||
|
- 持续复扫(persistent 默认开):找全后复扫到虚拟安全余量,
|
||||||
|
accounted_virtual_time_s 记录首次证明找全时刻;
|
||||||
|
- 网络故障幂等重试不污染闭环结果;
|
||||||
|
- 虚拟时刻逐步复算一致。
|
||||||
|
"""
|
||||||
|
|
||||||
|
import math
|
||||||
|
import unittest
|
||||||
|
from dataclasses import replace
|
||||||
|
|
||||||
|
from mcm26b.config import ProblemConfig
|
||||||
|
from mcm26b.evaluation import recompute_virtual_time
|
||||||
|
from mcm26b.planner.bayes_loop import BayesLoop
|
||||||
|
from mcm26b.protocol.client import SimulatorClient
|
||||||
|
from mcm26b.sim import (LocalSimTransport, LocalSimulator,
|
||||||
|
ScriptedFaultTransport, Source)
|
||||||
|
from mcm26b.sim.scenes import random_omni_scene
|
||||||
|
|
||||||
|
CONFIG = replace(ProblemConfig(),
|
||||||
|
search=replace(ProblemConfig().search,
|
||||||
|
bearing_rounding_margin_deg=0.005))
|
||||||
|
SMALL_CAP = replace(CONFIG,
|
||||||
|
field=replace(CONFIG.field, max_virtual_duration_s=60_000.0))
|
||||||
|
NO_PERSIST = replace(CONFIG, search=replace(CONFIG.search,
|
||||||
|
persistent_search=False))
|
||||||
|
|
||||||
|
|
||||||
|
def _run_scene(sources, fault_script=None, config=SMALL_CAP):
|
||||||
|
simulator = LocalSimulator(sources, robot_id="T001", config=config)
|
||||||
|
transport = LocalSimTransport(simulator)
|
||||||
|
if fault_script is not None:
|
||||||
|
transport = ScriptedFaultTransport(transport, fault_script)
|
||||||
|
client = SimulatorClient("T001", config=config, transport=transport)
|
||||||
|
loop = BayesLoop(client, config)
|
||||||
|
summary = loop.run()
|
||||||
|
report = recompute_virtual_time(client.log.action_events())
|
||||||
|
return summary, report, simulator
|
||||||
|
|
||||||
|
|
||||||
|
class BayesLoopAcceptanceTests(unittest.TestCase):
|
||||||
|
def test_ten_sources_all_cleared(self):
|
||||||
|
sources = random_omni_scene(seed=5, count=10, config=CONFIG)
|
||||||
|
summary, report, simulator = _run_scene(sources)
|
||||||
|
truth = sorted(s.channel for s in sources)
|
||||||
|
self.assertTrue(summary["completed"])
|
||||||
|
self.assertEqual(sorted(summary["cleared_channels"]), truth)
|
||||||
|
self.assertEqual(summary["unresolved_channels"], [])
|
||||||
|
self.assertIsNotNone(summary["accounted_virtual_time_s"])
|
||||||
|
self.assertEqual(summary["anomalies"], [], "守恒性或证书不应被打破")
|
||||||
|
self.assertEqual(report["mismatches"], [])
|
||||||
|
self.assertEqual(sorted(simulator.cleared), truth)
|
||||||
|
self.assertEqual(summary["exit_reason"], "user_exit")
|
||||||
|
self.assertEqual(summary["strategy"], "bayes")
|
||||||
|
|
||||||
|
def test_sixteen_sources_stop_by_count_bound(self):
|
||||||
|
sources = random_omni_scene(seed=6, count=16, config=CONFIG)
|
||||||
|
summary, report, simulator = _run_scene(sources)
|
||||||
|
truth = sorted(s.channel for s in sources)
|
||||||
|
self.assertTrue(summary["completed"])
|
||||||
|
self.assertEqual(summary["stop_reason"], "sixteen_cleared")
|
||||||
|
self.assertEqual(sorted(summary["cleared_channels"]), truth)
|
||||||
|
self.assertEqual(report["mismatches"], [])
|
||||||
|
self.assertEqual(summary["anomalies"], [])
|
||||||
|
|
||||||
|
def test_empty_arena_all_absent(self):
|
||||||
|
"""0 源:每个频道都必须由 PF 排除盘证明缺席后正常退出。"""
|
||||||
|
summary, report, simulator = _run_scene([], config=NO_PERSIST)
|
||||||
|
self.assertTrue(summary["completed"])
|
||||||
|
self.assertEqual(summary["stop_reason"], "all_resolved")
|
||||||
|
self.assertEqual(sorted(summary["absent_channels"]),
|
||||||
|
list(range(1, 21)))
|
||||||
|
self.assertEqual(summary["pf_absent_channels"], list(range(1, 21)))
|
||||||
|
self.assertEqual(summary["clears"], 0)
|
||||||
|
self.assertEqual(summary["anomalies"], [])
|
||||||
|
self.assertEqual(report["mismatches"], [])
|
||||||
|
|
||||||
|
def test_boundary_sources_detected_and_cleared(self):
|
||||||
|
"""近圆域边界源(r_eff=1000):七站覆盖半径 968.9m 保证发现。"""
|
||||||
|
sources = [Source(c + 1, 1800.0 * math.cos(math.pi / 6 + 2 * math.pi * c / 4),
|
||||||
|
1800.0 * math.sin(math.pi / 6 + 2 * math.pi * c / 4),
|
||||||
|
1000.0) for c in range(4)]
|
||||||
|
summary, report, simulator = _run_scene(sources, config=NO_PERSIST)
|
||||||
|
truth = sorted(s.channel for s in sources)
|
||||||
|
self.assertTrue(summary["completed"])
|
||||||
|
self.assertEqual(sorted(summary["cleared_channels"]), truth)
|
||||||
|
self.assertEqual(summary["anomalies"], [])
|
||||||
|
self.assertEqual(report["mismatches"], [])
|
||||||
|
|
||||||
|
def test_persistent_phase_resweeps_until_cap(self):
|
||||||
|
"""找全后持续复扫到虚拟安全余量 [用户决策 2026-09-11]。"""
|
||||||
|
sources = random_omni_scene(seed=5, count=10, config=CONFIG)
|
||||||
|
summary, _report, _simulator = _run_scene(sources, config=SMALL_CAP)
|
||||||
|
truth = sorted(s.channel for s in sources)
|
||||||
|
self.assertTrue(summary["completed"])
|
||||||
|
self.assertEqual(summary["stop_reason"], "virtual_time_cap")
|
||||||
|
self.assertEqual(sorted(summary["cleared_channels"]), truth)
|
||||||
|
self.assertGreater(summary["measures"], 7 * 20,
|
||||||
|
"找全后应继续复扫未清除频道")
|
||||||
|
|
||||||
|
def test_transport_fault_retries_keep_session_clean(self):
|
||||||
|
sources = random_omni_scene(seed=5, count=10, config=CONFIG)
|
||||||
|
script = [("drop_before", 1)] # 首个请求丢弃一次:幂等重试路径
|
||||||
|
summary, report, simulator = _run_scene(sources, fault_script=script)
|
||||||
|
truth = sorted(s.channel for s in sources)
|
||||||
|
self.assertTrue(summary["completed"])
|
||||||
|
self.assertEqual(sorted(summary["cleared_channels"]), truth)
|
||||||
|
self.assertEqual(report["mismatches"], [])
|
||||||
|
|
||||||
|
def test_skip_rule_never_skips_detectable_channel(self):
|
||||||
|
"""跳测规则只允许跳过"必 no_signal"的 (站,频道) 对。"""
|
||||||
|
sources = random_omni_scene(seed=5, count=10, config=CONFIG)
|
||||||
|
simulator = LocalSimulator(sources, robot_id="T001", config=CONFIG)
|
||||||
|
client = SimulatorClient("T001", config=CONFIG,
|
||||||
|
transport=LocalSimTransport(simulator))
|
||||||
|
loop = BayesLoop(client, CONFIG)
|
||||||
|
loop.run()
|
||||||
|
from mcm26b.belief import ChannelStatus
|
||||||
|
for index, point in enumerate(loop.ledger.points):
|
||||||
|
for channel, belief in loop.belief.channels.items():
|
||||||
|
if belief.status is not ChannelStatus.UNKNOWN:
|
||||||
|
continue
|
||||||
|
skipped = loop._skip_station_channel(channel, index, point)
|
||||||
|
if skipped and len(loop.particles[channel]):
|
||||||
|
nearest = loop.particles[channel].nearest_alive_distance(
|
||||||
|
point, loop.particles[channel].r_eff_max_m
|
||||||
|
+ loop.particles[channel].margin_m)
|
||||||
|
self.assertIsNone(
|
||||||
|
nearest, f"频道 {channel} 站点 {index} 被跳过但可能检出")
|
||||||
|
|
||||||
|
def test_cli_strategy_registry(self):
|
||||||
|
"""CLI --strategy 注册表:bayes/seven 用七站,grid 用 49 点网格。"""
|
||||||
|
import mcm26b.cli as cli
|
||||||
|
from mcm26b.coverage import coverage_points
|
||||||
|
from mcm26b.planner.seven import SevenBatch, seven_points
|
||||||
|
self.assertIs(cli.Q3_STRATEGIES["bayes"], BayesLoop)
|
||||||
|
for name, loop_cls in cli.Q3_STRATEGIES.items():
|
||||||
|
simulator = LocalSimulator([], robot_id="T001", config=CONFIG)
|
||||||
|
client = SimulatorClient("T001", config=CONFIG,
|
||||||
|
transport=LocalSimTransport(simulator))
|
||||||
|
loop = loop_cls(client, CONFIG)
|
||||||
|
expected = seven_points() if name in ("bayes", "seven") \
|
||||||
|
else coverage_points(CONFIG)
|
||||||
|
self.assertEqual(loop.ledger.points, expected)
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
unittest.main()
|
||||||
@@ -0,0 +1,172 @@
|
|||||||
|
"""P4b 网格粒子滤波验收:真源保留定理、证书正确性与查询原语。
|
||||||
|
|
||||||
|
核心不变式(docs/algo-3-bayes.md §2):
|
||||||
|
- 真源保留:任意观测序列(|误差| <= 1°、r_eff ∈ [1000,1500]、题面语义)
|
||||||
|
后,真源所在格必在幸存集中——由此 MEC 证书与缺席证明才严格成立;
|
||||||
|
- 缺席证明:无源频道在排除盘覆盖 Ω 后幸存集必须清空;
|
||||||
|
- 查询:nearest_alive_distance 与暴力扫描一致;version 单调递增。
|
||||||
|
"""
|
||||||
|
|
||||||
|
import math
|
||||||
|
import unittest
|
||||||
|
from dataclasses import replace
|
||||||
|
from random import Random
|
||||||
|
|
||||||
|
from mcm26b.config import ProblemConfig
|
||||||
|
from mcm26b.geometry.localization import smallest_enclosing_circle
|
||||||
|
from mcm26b.particles import ChannelParticles, angular_gap
|
||||||
|
from mcm26b.protocol.rules import DIRECTION, NEAR, NO_SIGNAL
|
||||||
|
from mcm26b.planner.seven import seven_points
|
||||||
|
|
||||||
|
CONFIG = ProblemConfig()
|
||||||
|
rng_shared = Random(99)
|
||||||
|
|
||||||
|
|
||||||
|
def _make_particle(spacing_m=25.0):
|
||||||
|
return ChannelParticles(CONFIG, spacing_m=spacing_m)
|
||||||
|
|
||||||
|
|
||||||
|
def _cell_index(particle, point):
|
||||||
|
i = int(math.floor((point[0] - particle.x0) / particle.spacing_m))
|
||||||
|
j = int(math.floor((point[1] - particle.x0) / particle.spacing_m))
|
||||||
|
return i * particle.width + j
|
||||||
|
|
||||||
|
|
||||||
|
def _observe(source, station, rng, cleared=False):
|
||||||
|
"""按题面语义产生一次读数(误差在 [-1°,1°] 内随机,含两位小数舍入)。"""
|
||||||
|
if cleared:
|
||||||
|
return NO_SIGNAL, None
|
||||||
|
dist = math.dist(station, (source[0], source[1]))
|
||||||
|
if dist > source[2]:
|
||||||
|
return NO_SIGNAL, None
|
||||||
|
if dist <= CONFIG.field.near_radius_m:
|
||||||
|
return NEAR, None
|
||||||
|
true_bearing = math.degrees(math.atan2(source[1] - station[1],
|
||||||
|
source[0] - station[0]))
|
||||||
|
error = (2.0 * rng.random() - 1.0) * CONFIG.field.bearing_error_half_deg
|
||||||
|
return DIRECTION, round(true_bearing + error, 2) % 360.0
|
||||||
|
|
||||||
|
|
||||||
|
class TruthPreservationTests(unittest.TestCase):
|
||||||
|
def test_truth_cell_survives_arbitrary_sequences(self):
|
||||||
|
"""随机源 × 随机测点 × 随机合法误差:真源所在格永不被删。"""
|
||||||
|
rng = Random(20260911)
|
||||||
|
for trial in range(8):
|
||||||
|
particle = _make_particle()
|
||||||
|
radius = CONFIG.field.omega_radius_m * math.sqrt(rng.random())
|
||||||
|
angle = rng.uniform(0.0, 2.0 * math.pi)
|
||||||
|
source = (radius * math.cos(angle), radius * math.sin(angle),
|
||||||
|
rng.uniform(1000.0, 1500.0))
|
||||||
|
truth_cell = _cell_index(particle, source)
|
||||||
|
self.assertIn(truth_cell, particle.alive)
|
||||||
|
stations = [(rng.uniform(-1800, 1800), rng.uniform(-1800, 1800))
|
||||||
|
for _ in range(25)] + seven_points()
|
||||||
|
for station in stations:
|
||||||
|
result, svd = _observe(source, station, rng)
|
||||||
|
particle.apply_measure(station, result, svd)
|
||||||
|
self.assertIn(truth_cell, particle.alive,
|
||||||
|
f"trial {trial} station {station}: 真源格被删")
|
||||||
|
self.assertTrue(particle.alive,
|
||||||
|
"含源频道幸存集不允许为空")
|
||||||
|
# 真源距幸存集 <= Voronoi 裕量(清除证书的几何依据)
|
||||||
|
nearest = min(math.dist(p, source[:2]) for p in
|
||||||
|
(particle.cell_center(i) for i in particle.alive))
|
||||||
|
self.assertLessEqual(nearest, particle.margin_m + 1e-6)
|
||||||
|
|
||||||
|
def test_absence_proof_when_exclusions_cover_domain(self):
|
||||||
|
"""无源频道:七站 no_signal 的排除盘覆盖 Ω,幸存集必须清空。"""
|
||||||
|
particle = _make_particle()
|
||||||
|
for station in seven_points():
|
||||||
|
particle.apply_measure(station, NO_SIGNAL, None)
|
||||||
|
self.assertEqual(len(particle), 0,
|
||||||
|
"七站覆盖半径 968.9m < 1000−m,缺席必须可证")
|
||||||
|
|
||||||
|
def test_direction_result_never_empties_feasible_set(self):
|
||||||
|
"""direction 分支(角域 ∩ B(s,1500))不允许把含源频道剪空。"""
|
||||||
|
rng = Random(7)
|
||||||
|
particle = _make_particle()
|
||||||
|
source = (900.0, -700.0, 1200.0)
|
||||||
|
for _ in range(40):
|
||||||
|
station = (rng.uniform(-1700, 1700), rng.uniform(-1700, 1700))
|
||||||
|
result, svd = _observe(source, station, rng)
|
||||||
|
particle.apply_measure(station, result, svd)
|
||||||
|
if result == DIRECTION:
|
||||||
|
self.assertTrue(particle.alive)
|
||||||
|
|
||||||
|
|
||||||
|
class CertificateTests(unittest.TestCase):
|
||||||
|
def test_clear_certificate_implies_clear_success(self):
|
||||||
|
"""证书蕴含关系:MEC(幸存)+裕量 <= 20−0.5 ⟹ 真源距清除点 <= 20。
|
||||||
|
|
||||||
|
用小测试场(omega=200m)+ 细网格(h=5m)使证书可达:两次方向
|
||||||
|
交会后 near 读数把可行集收缩到 5m 盘,网格证书触发,然后按
|
||||||
|
/clear 语义验证真源到证书圆心的距离不超过 20m。
|
||||||
|
"""
|
||||||
|
small = replace(CONFIG, field=replace(CONFIG.field,
|
||||||
|
omega_radius_m=200.0))
|
||||||
|
particle = ChannelParticles(small, spacing_m=5.0)
|
||||||
|
limit = (CONFIG.field.clear_radius_m - particle.margin_m
|
||||||
|
- CONFIG.search.clear_margin_m)
|
||||||
|
source = (60.0, 40.0, 1500.0) # r_eff 取题面上界;小场内必收得到信号
|
||||||
|
# 两个远站方向交会(读数带 ±1° 合法误差),可行集收缩为小菱形
|
||||||
|
for station in ((-150.0, -40.0), (40.0, -150.0)):
|
||||||
|
result, svd = _observe(source, station, rng_shared)
|
||||||
|
particle.apply_measure(station, result, svd)
|
||||||
|
self.assertTrue(particle.alive)
|
||||||
|
# near:源距站 3m(<=5m),可行集收缩到 B(station, 5+裕量)
|
||||||
|
station = (source[0] + 3.0, source[1])
|
||||||
|
particle.apply_measure(station, NEAR, None)
|
||||||
|
center, radius = particle.mec()
|
||||||
|
self.assertLessEqual(radius, limit,
|
||||||
|
"5m 盘收缩后网格证书必须可触发")
|
||||||
|
self.assertLessEqual(radius + particle.margin_m
|
||||||
|
+ CONFIG.search.clear_margin_m,
|
||||||
|
CONFIG.field.clear_radius_m)
|
||||||
|
self.assertLessEqual(math.dist(center, source[:2]),
|
||||||
|
CONFIG.field.clear_radius_m,
|
||||||
|
"证书触发时真源到清除点必须 <= 20m")
|
||||||
|
|
||||||
|
def test_mec_contains_all_alive_cells(self):
|
||||||
|
particle = _make_particle()
|
||||||
|
particle.apply_measure((400.0, 300.0), DIRECTION, 25.0)
|
||||||
|
center, radius = particle.mec()
|
||||||
|
for point in particle.alive_points():
|
||||||
|
self.assertLessEqual(math.dist(point, center), radius + 1e-6)
|
||||||
|
|
||||||
|
|
||||||
|
class QueryPrimitiveTests(unittest.TestCase):
|
||||||
|
def test_nearest_alive_distance_matches_bruteforce(self):
|
||||||
|
rng = Random(3)
|
||||||
|
particle = _make_particle()
|
||||||
|
for _ in range(6):
|
||||||
|
station = (rng.uniform(-1500, 1500), rng.uniform(-1500, 1500))
|
||||||
|
particle.apply_measure(station, NO_SIGNAL, None)
|
||||||
|
for _ in range(30):
|
||||||
|
probe = (rng.uniform(-1900, 1900), rng.uniform(-1900, 1900))
|
||||||
|
limit = rng.uniform(100.0, 2500.0)
|
||||||
|
got = particle.nearest_alive_distance(probe, limit)
|
||||||
|
brute = min((math.dist(p, probe) for p in particle.alive_points()),
|
||||||
|
default=None)
|
||||||
|
if brute is not None and brute <= limit:
|
||||||
|
self.assertIsNotNone(got)
|
||||||
|
self.assertAlmostEqual(got, brute, delta=1e-6)
|
||||||
|
else:
|
||||||
|
self.assertIsNone(got,
|
||||||
|
f"limit={limit} brute={brute} 应返回 None")
|
||||||
|
|
||||||
|
def test_version_and_mec_cache(self):
|
||||||
|
particle = _make_particle()
|
||||||
|
before = particle.version
|
||||||
|
particle.apply_measure((0.0, 0.0), NO_SIGNAL, None)
|
||||||
|
self.assertGreater(particle.version, before)
|
||||||
|
mec_once = particle.mec()
|
||||||
|
self.assertIs(particle.mec(), mec_once, "version 不变时 MEC 应命中缓存")
|
||||||
|
|
||||||
|
def test_angular_gap(self):
|
||||||
|
self.assertAlmostEqual(angular_gap(350.0, 10.0), 20.0)
|
||||||
|
self.assertAlmostEqual(angular_gap(10.0, 350.0), 20.0)
|
||||||
|
self.assertAlmostEqual(angular_gap(45.0, 45.0), 0.0)
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
unittest.main()
|
||||||
Reference in New Issue
Block a user