125 lines
5.6 KiB
Python
125 lines
5.6 KiB
Python
"""第四问 (位置 × 朝向) 联合信念验收（docs/第三四问图论与动态规划优化方案.md §3.1）。
|
||
|
||
要点：
|
||
- 真源 (位置, 朝向) 在与真值一致的观测序列后**永不**被删（保守性二维推广）；
|
||
- 期望后验 MEC 半径是合法的选点值函数：远处被朝向遮挡的 no_signal 不删
|
||
**空间**候选，因此不应被误当成"已经收缩"；
|
||
- numpy 向量路径与纯 Python 回退路径在同一输入下结果一致。
|
||
"""
|
||
|
||
import math
|
||
import unittest
|
||
from dataclasses import replace
|
||
from random import Random
|
||
|
||
from mcm26b.belief.joint import JointBelief, _np
|
||
from mcm26b.config import ProblemConfig
|
||
from mcm26b.protocol.rules import DIRECTION, NEAR, NO_SIGNAL
|
||
|
||
CONFIG = replace(ProblemConfig(),
|
||
search=replace(ProblemConfig().search,
|
||
bearing_rounding_margin_deg=0.005))
|
||
HALF_ANGLE = CONFIG.field.directional_half_angle_deg
|
||
|
||
|
||
def _visible(source, heading, reader, limit_m):
|
||
if math.dist(reader, source) > limit_m:
|
||
return False
|
||
bearing = math.degrees(math.atan2(reader[1] - source[1],
|
||
reader[0] - source[0]))
|
||
return abs((bearing - heading + 180.0) % 360.0 - 180.0) <= HALF_ANGLE - 5.0
|
||
|
||
|
||
class JointBeliefRetentionTests(unittest.TestCase):
|
||
def test_true_source_survives_consistent_observations(self):
|
||
"""与真值一致的观测序列后，真源所在格仍必须存在。"""
|
||
rng = Random(11)
|
||
checked = 0
|
||
while checked < 10:
|
||
source = (rng.uniform(-1700, 1700), rng.uniform(-1700, 1700))
|
||
if math.hypot(*source) > 1700:
|
||
continue
|
||
heading = rng.uniform(0.0, 360.0)
|
||
detection = (rng.uniform(-2100, 2100), rng.uniform(-2100, 2100))
|
||
if not _visible(source, heading, detection,
|
||
CONFIG.field.r_eff_min_m):
|
||
continue
|
||
checked += 1
|
||
joint = JointBelief(CONFIG)
|
||
bearing = math.degrees(math.atan2(source[1] - detection[1],
|
||
source[0] - detection[0])) % 360.0
|
||
joint.apply(detection, DIRECTION,
|
||
round(bearing + rng.uniform(-0.9, 0.9), 2) % 360.0)
|
||
for _ in range(5):
|
||
probe = (rng.uniform(-1800, 1800), rng.uniform(-1800, 1800))
|
||
if _visible(source, heading, probe, CONFIG.field.r_eff_min_m):
|
||
pb = math.degrees(math.atan2(source[1] - probe[1],
|
||
source[0] - probe[0])) % 360.0
|
||
joint.apply(probe, DIRECTION,
|
||
round(pb + rng.uniform(-0.9, 0.9), 2) % 360.0)
|
||
else:
|
||
joint.apply(probe, NO_SIGNAL)
|
||
self.assertFalse(joint.is_absent(), "真源可行集被观测清空")
|
||
assert_source = (round(source[0], 6), round(source[1], 6))
|
||
nearest = min((math.dist(joint.position.cell_center(index), assert_source)
|
||
for index in joint.position.alive),
|
||
default=float("inf"))
|
||
self.assertLessEqual(nearest, joint.position.margin_m + 1e-6,
|
||
"真源所在格被联合信念删除")
|
||
|
||
def test_repeated_directions_shrink_but_never_grow(self):
|
||
"""方向读数只会收缩联合集；贴测点的格按保守设计保留。"""
|
||
joint = JointBelief(CONFIG)
|
||
joint.apply((0.0, 0.0), DIRECTION, 0.0)
|
||
first = len(joint)
|
||
joint.apply((0.0, 0.0), DIRECTION, 180.0)
|
||
self.assertLessEqual(len(joint), first)
|
||
self.assertGreater(len(joint), 0)
|
||
|
||
def test_near_keeps_only_a_small_disk(self):
|
||
joint = JointBelief(CONFIG)
|
||
joint.apply((100.0, 100.0), NEAR)
|
||
center, radius = joint.position.mec()
|
||
# 观测更新内缩 Voronoi 裕量，故上界为 near + margin
|
||
self.assertLessEqual(radius, CONFIG.field.near_radius_m
|
||
+ joint.position.margin_m + 1.0)
|
||
self.assertLess(math.dist(center, (100.0, 100.0)), 15.0)
|
||
|
||
|
||
class ExpectedPosteriorRadiusTests(unittest.TestCase):
|
||
def _joint(self):
|
||
joint = JointBelief(CONFIG)
|
||
joint.apply((0.0, 0.0), DIRECTION, 0.0)
|
||
return joint
|
||
|
||
def test_value_is_finite_and_positive(self):
|
||
joint = self._joint()
|
||
for probe in ((800.0, 0.0), (0.0, 800.0), (-500.0, -500.0)):
|
||
value = joint.expected_posterior_radius(probe)
|
||
self.assertTrue(math.isfinite(value))
|
||
self.assertGreater(value, 0.0)
|
||
|
||
def test_blocked_reading_keeps_spatial_radius(self):
|
||
"""no_signal 不删空间候选：期望半径不应被误当作已收缩。"""
|
||
joint = self._joint()
|
||
before = joint.position.mec()[1]
|
||
joint.apply((1500.0, 0.0), NO_SIGNAL)
|
||
self.assertAlmostEqual(joint.position.mec()[1], before, places=6)
|
||
self.assertGreater(joint.expected_posterior_radius((1500.0, 0.0)),
|
||
0.5 * before)
|
||
|
||
def test_vector_and_python_paths_agree(self):
|
||
if _np is None:
|
||
self.skipTest("环境无 numpy：纯 Python 路径即默认实现")
|
||
joint = self._joint()
|
||
for probe in ((600.0, 100.0), (-300.0, 700.0)):
|
||
indices = joint.sample(200)
|
||
representatives = indices[::max(len(indices) // 7, 1)]
|
||
vector = joint._epr_vector(probe, indices, representatives)
|
||
python = joint._epr_python(probe, indices, representatives)
|
||
self.assertAlmostEqual(vector, python, places=6)
|
||
|
||
|
||
if __name__ == "__main__":
|
||
unittest.main()
|