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,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