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:
@@ -14,80 +14,20 @@ from pathlib import Path
|
||||
from statistics import mean, median
|
||||
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.coverage import CoverageLedger
|
||||
from mcm26b.planner import OmnidirectionalLoop, clear_certificate
|
||||
from mcm26b.coverage import CoverageLedger # noqa: F401
|
||||
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.rules import NEAR
|
||||
from mcm26b.protocol.rules import NEAR # noqa: F401
|
||||
from mcm26b.sim import LocalSimulator, LocalSimTransport, Source
|
||||
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]
|
||||
|
||||
|
||||
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,
|
||||
"seven_batch": SevenBatch, "seven_sweep": SevenSweep}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user