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:
Horldsence
@@ -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