aurasync-engine 0.4.1__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- aurasync/__init__.py +0 -0
- aurasync/conditions.py +84 -0
- aurasync/controller.py +36 -0
- aurasync/estimator.py +251 -0
- aurasync/event_camera.py +196 -0
- aurasync/frame_camera.py +163 -0
- aurasync/imu.py +98 -0
- aurasync/monitor.py +80 -0
- aurasync/physics.py +62 -0
- aurasync/safety.py +173 -0
- aurasync/scheduler.py +191 -0
- aurasync/simulate.py +168 -0
- aurasync/v4/__init__.py +0 -0
- aurasync/v4/geo_pipeline.py +61 -0
- aurasync/v4/geometry.py +97 -0
- aurasync/v4/pipeline.py +274 -0
- aurasync/v41/__init__.py +0 -0
- aurasync/v41/pipeline.py +269 -0
- aurasync_engine-0.4.1.dist-info/METADATA +132 -0
- aurasync_engine-0.4.1.dist-info/RECORD +23 -0
- aurasync_engine-0.4.1.dist-info/WHEEL +5 -0
- aurasync_engine-0.4.1.dist-info/licenses/LICENSE +21 -0
- aurasync_engine-0.4.1.dist-info/top_level.txt +1 -0
aurasync/scheduler.py
ADDED
|
@@ -0,0 +1,191 @@
|
|
|
1
|
+
"""Event-triggered neural correction with a deadline guarantee (Upgrade C).
|
|
2
|
+
|
|
3
|
+
Analogy: an air-traffic controller who dead-reckons every plane each second
|
|
4
|
+
(cheap) and only asks for a radar sweep (expensive) when something happens: a
|
|
5
|
+
burst of activity, growing doubt, a plane near another one, or simply because it
|
|
6
|
+
has been too long. The radar takes time, so the controller never waits for it.
|
|
7
|
+
|
|
8
|
+
Per 1 ms control tick (never waits on the network):
|
|
9
|
+
1. physics prediction s <- RK4(s, u); covariance Sigma <- A Sigma A^T + Q
|
|
10
|
+
2. if a correction result has arrived: roll the corrected state at its capture
|
|
11
|
+
time forward through the controls applied since (replay), take its covariance
|
|
12
|
+
3. if the worker is idle and a trigger fires, capture the current time surface
|
|
13
|
+
and start a correction job; it completes after a compute latency sampled
|
|
14
|
+
from the latency measured on this CPU
|
|
15
|
+
Triggers (mode 'triggered'):
|
|
16
|
+
burst >= burst_thr pixels fired an event in the last tick
|
|
17
|
+
doubt propagated sigma_theta > sigma_trig
|
|
18
|
+
margin |psi| = |theta + 0.1 theta_dot| > margin_trig (close to the safety envelope)
|
|
19
|
+
max gap >= max_gap ticks since the last correction (quiet scenes still get checked)
|
|
20
|
+
Deadline: a job not finished within `deadline` ticks is abandoned (deadline
|
|
21
|
+
miss, logged). Fallback is deterministic: keep the physics prediction and let
|
|
22
|
+
the covariance grow, which tightens the safety shield and can trip its stale mode.
|
|
23
|
+
Other modes, for comparison at matched compute:
|
|
24
|
+
dense a correction every tick, synchronous, 1 tick delay (the v2 behaviour;
|
|
25
|
+
infeasible in real time on this CPU, kept as reference)
|
|
26
|
+
greedy async; start a new job whenever the worker is free
|
|
27
|
+
periodic async; start a job every `period` ticks
|
|
28
|
+
"""
|
|
29
|
+
from collections import deque
|
|
30
|
+
import numpy as np
|
|
31
|
+
import torch
|
|
32
|
+
|
|
33
|
+
from .physics import PlantParams, rk4_step, linearise
|
|
34
|
+
from .estimator import SCALE
|
|
35
|
+
|
|
36
|
+
DT = 1e-3
|
|
37
|
+
SCALE_NP = SCALE.numpy().astype(np.float64)
|
|
38
|
+
FRESH = float(np.exp(-1e-3 / 0.008)) # fast time-surface value of an event < 1 ms old
|
|
39
|
+
|
|
40
|
+
|
|
41
|
+
class TriggeredEventPipeline:
|
|
42
|
+
def __init__(self, models, mode="triggered", latency_samples=(0.0018,), deadline=6,
|
|
43
|
+
period=5, max_gap=20, burst_thr=12, sigma_trig_deg=1.5, margin_trig_deg=7.0,
|
|
44
|
+
temp=None, Q=None, seed=0, name=None, plant=None):
|
|
45
|
+
self.models = [m.eval() for m in (models if isinstance(models, (list, tuple)) else [models])]
|
|
46
|
+
self.mode, self.deadline, self.period, self.max_gap = mode, deadline, period, max_gap
|
|
47
|
+
self.burst_thr = burst_thr
|
|
48
|
+
self.sigma_trig, self.margin_trig = np.radians(sigma_trig_deg), np.radians(margin_trig_deg)
|
|
49
|
+
self.lat = np.asarray(latency_samples, float) * len(self.models) # members run sequentially
|
|
50
|
+
self.temp = np.ones(4) if temp is None else np.asarray(temp, float)
|
|
51
|
+
self.p = plant or PlantParams()
|
|
52
|
+
self.A, _ = linearise(self.p, DT)
|
|
53
|
+
self.Q = np.diag([1e-9, 1e-5, 1e-8, 1e-4]) if Q is None else Q
|
|
54
|
+
self.seed = seed
|
|
55
|
+
tag = "" if len(self.models) == 1 else f"-ens{len(self.models)}"
|
|
56
|
+
self.name = name or f"event-{mode}{tag}"
|
|
57
|
+
self.delay_ticks = 1 if mode == "dense" else 0
|
|
58
|
+
|
|
59
|
+
# ---- interface used by run_episode / ShieldedController ---------------------
|
|
60
|
+
def reset(self, s0):
|
|
61
|
+
self.rng = np.random.default_rng(self.seed)
|
|
62
|
+
self.s = np.asarray(s0, float).copy()
|
|
63
|
+
self.Sigma = np.diag((0.05 * SCALE_NP) ** 2)
|
|
64
|
+
self.h = [m.init_hidden(1) for m in self.models]
|
|
65
|
+
self.u_hist = deque(maxlen=256)
|
|
66
|
+
self.k = -1
|
|
67
|
+
self.last_capture = 0
|
|
68
|
+
self.job = None # (capture_k, done_k, s_corr, Sigma_corr, new_h)
|
|
69
|
+
self.out = deque([(self.s.copy(), self.Sigma.copy())] * 2, maxlen=2)
|
|
70
|
+
self.log = dict(calls=0, misses=0, triggers=dict(burst=0, doubt=0, margin=0, gap=0, idle=0, periodic=0),
|
|
71
|
+
busy_ticks=0, sigma_th=[], corrected=[])
|
|
72
|
+
|
|
73
|
+
def estimate(self):
|
|
74
|
+
return self.out[0][0] if self.mode == "dense" else self.out[-1][0]
|
|
75
|
+
|
|
76
|
+
def covariance(self):
|
|
77
|
+
return self.out[0][1] if self.mode == "dense" else self.out[-1][1]
|
|
78
|
+
|
|
79
|
+
def age_ticks(self):
|
|
80
|
+
return self.delay_ticks
|
|
81
|
+
|
|
82
|
+
# ---- internals ----------------------------------------------------------------
|
|
83
|
+
@torch.inference_mode()
|
|
84
|
+
def _correct(self, surface, s_pred, gap):
|
|
85
|
+
ts = torch.from_numpy(np.ascontiguousarray(surface, dtype=np.float32))[None]
|
|
86
|
+
sp = torch.tensor(s_pred, dtype=torch.float32)[None]
|
|
87
|
+
g = torch.tensor([float(gap)])
|
|
88
|
+
outs, vars_, hs = [], [], []
|
|
89
|
+
for m, h in zip(self.models, self.h):
|
|
90
|
+
z, act = m.encode(ts)
|
|
91
|
+
s_new, h_new, _, lv = m.correct(z, act, h, sp, g)
|
|
92
|
+
outs.append(s_new[0].double().numpy()); hs.append(h_new)
|
|
93
|
+
vars_.append((np.exp(lv[0].double().numpy()) * SCALE_NP ** 2) * self.temp ** 2)
|
|
94
|
+
outs = np.stack(outs)
|
|
95
|
+
mean = outs.mean(0)
|
|
96
|
+
var = np.mean(vars_, 0) + (outs.var(0) if len(outs) > 1 else 0.0) # aleatoric + epistemic
|
|
97
|
+
return mean, np.diag(var), hs
|
|
98
|
+
|
|
99
|
+
def _propagate(self, s, Sigma, u):
|
|
100
|
+
return rk4_step(s, u, DT, self.p), self.A @ Sigma @ self.A.T + self.Q
|
|
101
|
+
|
|
102
|
+
def _trigger(self, surface):
|
|
103
|
+
if self.mode == "greedy":
|
|
104
|
+
return "idle"
|
|
105
|
+
gap = self.k - self.last_capture
|
|
106
|
+
if self.mode == "periodic":
|
|
107
|
+
return "periodic" if gap >= self.period else None
|
|
108
|
+
if gap >= self.max_gap:
|
|
109
|
+
return "gap"
|
|
110
|
+
if (surface[0] >= FRESH).sum() + (surface[1] >= FRESH).sum() >= self.burst_thr:
|
|
111
|
+
return "burst"
|
|
112
|
+
if np.sqrt(self.Sigma[2, 2]) > self.sigma_trig:
|
|
113
|
+
return "doubt"
|
|
114
|
+
if abs(self.s[2] + 0.1 * self.s[3]) > self.margin_trig:
|
|
115
|
+
return "margin"
|
|
116
|
+
return None
|
|
117
|
+
|
|
118
|
+
def update(self, k, t, s_true, surface, u):
|
|
119
|
+
self.k = k
|
|
120
|
+
self.u_hist.append(u)
|
|
121
|
+
if self.mode == "dense":
|
|
122
|
+
s_pred, _ = self._propagate(self.s, self.Sigma, u)
|
|
123
|
+
self.s, self.Sigma, self.h = self._correct(surface, s_pred, 1)
|
|
124
|
+
self.log["calls"] += 1
|
|
125
|
+
self.out.append((self.s.copy(), self.Sigma.copy()))
|
|
126
|
+
self.log["sigma_th"].append(np.sqrt(self.Sigma[2, 2])); self.log["corrected"].append(True)
|
|
127
|
+
return
|
|
128
|
+
# 1. cheap physics prediction, every tick
|
|
129
|
+
self.s, self.Sigma = self._propagate(self.s, self.Sigma, u)
|
|
130
|
+
corrected = False
|
|
131
|
+
# 2. collect a finished job (or abandon one past its deadline)
|
|
132
|
+
if self.job is not None:
|
|
133
|
+
cap_k, done_k, s_c, S_c, h_new = self.job
|
|
134
|
+
if done_k - cap_k > self.deadline and k >= cap_k + self.deadline:
|
|
135
|
+
self.log["misses"] += 1 # deadline miss -> discard
|
|
136
|
+
self.job = None
|
|
137
|
+
elif k >= done_k:
|
|
138
|
+
for uu in list(self.u_hist)[-(k - cap_k):] if k > cap_k else []:
|
|
139
|
+
s_c, S_c = self._propagate(s_c, S_c, uu) # replay controls since capture
|
|
140
|
+
self.s, self.Sigma, self.h = s_c, S_c, h_new
|
|
141
|
+
self.job = None
|
|
142
|
+
corrected = True
|
|
143
|
+
else:
|
|
144
|
+
self.log["busy_ticks"] += 1
|
|
145
|
+
# 3. start a new job if idle and triggered
|
|
146
|
+
if self.job is None:
|
|
147
|
+
why = self._trigger(surface)
|
|
148
|
+
if why is not None:
|
|
149
|
+
gap = max(1, k - self.last_capture)
|
|
150
|
+
s_c, S_c, h_new = self._correct(surface, self.s, gap)
|
|
151
|
+
lat_ticks = int(np.ceil(self.rng.choice(self.lat) / DT))
|
|
152
|
+
self.job = (k, k + lat_ticks, s_c, S_c, h_new)
|
|
153
|
+
self.last_capture = k
|
|
154
|
+
self.log["calls"] += 1
|
|
155
|
+
self.log["triggers"][why] += 1
|
|
156
|
+
self.out.append((self.s.copy(), self.Sigma.copy()))
|
|
157
|
+
self.log["sigma_th"].append(np.sqrt(self.Sigma[2, 2])); self.log["corrected"].append(corrected)
|
|
158
|
+
|
|
159
|
+
|
|
160
|
+
@torch.inference_mode()
|
|
161
|
+
def measure_correction_latency(model, n=3000, warm=200):
|
|
162
|
+
"""Wall-clock cost of one neural correction (encode + correct) on one CPU thread."""
|
|
163
|
+
import time
|
|
164
|
+
torch.set_num_threads(1)
|
|
165
|
+
ts = torch.rand(1, 4, 32, 96); h = model.init_hidden(1); s = torch.zeros(1, 4); g = torch.ones(1)
|
|
166
|
+
out = []
|
|
167
|
+
for i in range(n + warm):
|
|
168
|
+
a = time.perf_counter()
|
|
169
|
+
z, act = model.encode(ts)
|
|
170
|
+
model.correct(z, act, h, s, g)
|
|
171
|
+
b = time.perf_counter()
|
|
172
|
+
if i >= warm:
|
|
173
|
+
out.append(b - a)
|
|
174
|
+
return np.array(out)
|
|
175
|
+
|
|
176
|
+
|
|
177
|
+
def physics_tick_cost(n=5000):
|
|
178
|
+
"""Cost of the always-on part of the control tick: prediction + covariance + LQR + shield."""
|
|
179
|
+
import time
|
|
180
|
+
from .controller import LQRController
|
|
181
|
+
from .safety import SafetyShield
|
|
182
|
+
p = PlantParams(); A, _ = linearise(p, DT); Q = np.diag([1e-9, 1e-5, 1e-8, 1e-4])
|
|
183
|
+
lqr = LQRController(p); sh = SafetyShield(p)
|
|
184
|
+
s = np.array([0.01, 0.0, 0.02, 0.1]); S = np.eye(4) * 1e-4
|
|
185
|
+
out = []
|
|
186
|
+
for i in range(n):
|
|
187
|
+
a = time.perf_counter()
|
|
188
|
+
s2 = rk4_step(s, 0.5, DT, p); S2 = A @ S @ A.T + Q
|
|
189
|
+
sh.filter(s2, S2, lqr.act(s2))
|
|
190
|
+
out.append(time.perf_counter() - a)
|
|
191
|
+
return np.array(out)
|
aurasync/simulate.py
ADDED
|
@@ -0,0 +1,168 @@
|
|
|
1
|
+
"""Closed-loop hardware-in-the-loop style simulation.
|
|
2
|
+
|
|
3
|
+
Plant physics runs at 5 kHz (200 us substeps); the event camera samples every
|
|
4
|
+
substep; the controller ticks at 1 kHz. Perception pipelines plug in through a
|
|
5
|
+
small interface so event-based and frame-based stacks are compared on the
|
|
6
|
+
exact same plant, disturbances and controller.
|
|
7
|
+
"""
|
|
8
|
+
from collections import deque
|
|
9
|
+
import numpy as np
|
|
10
|
+
import torch
|
|
11
|
+
|
|
12
|
+
from .physics import rk4_step, failed
|
|
13
|
+
from .event_camera import SceneRenderer, EventCamera, TimeSurfaceEncoder
|
|
14
|
+
from .conditions import NOMINAL
|
|
15
|
+
|
|
16
|
+
DT = 1e-3
|
|
17
|
+
SUB = 5
|
|
18
|
+
|
|
19
|
+
|
|
20
|
+
class OraclePipeline:
|
|
21
|
+
"""Perfect, instantaneous state (upper bound - not physically achievable)."""
|
|
22
|
+
name = "oracle"
|
|
23
|
+
|
|
24
|
+
def reset(self, s0): self.s = s0.copy()
|
|
25
|
+
|
|
26
|
+
def update(self, k, t, s_true, surface, u): self.s = s_true.copy()
|
|
27
|
+
|
|
28
|
+
def estimate(self): return self.s
|
|
29
|
+
|
|
30
|
+
|
|
31
|
+
class FramePipeline:
|
|
32
|
+
"""Conventional frame camera + vision model.
|
|
33
|
+
|
|
34
|
+
A frame is captured every 1/fps seconds; its result is available `latency`
|
|
35
|
+
seconds later (readout + inference). Deliberately generous: the frame model
|
|
36
|
+
is given the TRUE pose and velocity at capture time (no estimation error,
|
|
37
|
+
no motion blur). Only the timing is realistic.
|
|
38
|
+
"""
|
|
39
|
+
|
|
40
|
+
def __init__(self, fps, latency):
|
|
41
|
+
self.fps, self.latency = fps, latency
|
|
42
|
+
self.name = f"frame-{fps}fps"
|
|
43
|
+
|
|
44
|
+
def reset(self, s0):
|
|
45
|
+
self.pending = deque()
|
|
46
|
+
self.s = s0.copy()
|
|
47
|
+
self.next_capture = 0.0
|
|
48
|
+
|
|
49
|
+
def update(self, k, t, s_true, surface, u):
|
|
50
|
+
if t + 1e-9 >= self.next_capture:
|
|
51
|
+
self.pending.append((t + self.latency, s_true.copy()))
|
|
52
|
+
self.next_capture += 1.0 / self.fps
|
|
53
|
+
while self.pending and self.pending[0][0] <= t + 1e-9:
|
|
54
|
+
self.s = self.pending.popleft()[1]
|
|
55
|
+
|
|
56
|
+
def estimate(self): return self.s
|
|
57
|
+
|
|
58
|
+
|
|
59
|
+
class EventPipeline:
|
|
60
|
+
"""AuraSync: event time-surface -> physics-informed observer, 1 tick latency."""
|
|
61
|
+
name = "aurasync-event"
|
|
62
|
+
|
|
63
|
+
def __init__(self, model, delay_ticks=1):
|
|
64
|
+
self.model = model.eval()
|
|
65
|
+
self.delay_ticks = delay_ticks
|
|
66
|
+
|
|
67
|
+
def reset(self, s0):
|
|
68
|
+
self.s_hat = torch.tensor(s0, dtype=torch.float32)[None]
|
|
69
|
+
self.h = self.model.init_hidden(1)
|
|
70
|
+
self.buf = deque([s0.copy()] * (self.delay_ticks + 1), maxlen=self.delay_ticks + 1)
|
|
71
|
+
|
|
72
|
+
@torch.inference_mode()
|
|
73
|
+
def update(self, k, t, s_true, surface, u):
|
|
74
|
+
z = self.model.enc(torch.from_numpy(surface)[None])
|
|
75
|
+
self.s_hat, self.h, _ = self.model.cell(z, self.h, self.s_hat,
|
|
76
|
+
torch.tensor([u], dtype=torch.float32))
|
|
77
|
+
self.buf.append(self.s_hat[0].numpy().astype(np.float64))
|
|
78
|
+
|
|
79
|
+
def estimate(self): return self.buf[0]
|
|
80
|
+
|
|
81
|
+
|
|
82
|
+
def run_episode(p, controller, pipeline, n_ticks, s0, kicks=(), snap=None,
|
|
83
|
+
exploration=None, rng=None, record=False, cam_seed=0,
|
|
84
|
+
stop_on_fail=True, renderer=None, aps_fps=30, aps_latency=0.005, cond=NOMINAL,
|
|
85
|
+
aps_seed=None):
|
|
86
|
+
"""kicks: list of (tick, delta_theta_dot). snap: (tick, new_params) mid-episode
|
|
87
|
+
plant change (e.g. mass shift from a mechanical failure).
|
|
88
|
+
|
|
89
|
+
The sensor is modelled as a hybrid DAVIS-style pixel array: asynchronous
|
|
90
|
+
events PLUS a slow absolute intensity (APS) frame at `aps_fps`, delivered
|
|
91
|
+
`aps_latency` after capture. Events give speed; the slow frame anchors
|
|
92
|
+
drift when the scene is still and no events are produced.
|
|
93
|
+
|
|
94
|
+
cond: SensorConditions shared by the scene, the event camera, its APS frame
|
|
95
|
+
and any frame-camera pipeline (NOMINAL reproduces v2 exactly)."""
|
|
96
|
+
nominal = cond.is_nominal
|
|
97
|
+
renderer = renderer or SceneRenderer(cond=cond)
|
|
98
|
+
if nominal:
|
|
99
|
+
cam = EventCamera(renderer.H, renderer.W, seed=cam_seed)
|
|
100
|
+
else:
|
|
101
|
+
cam = EventCamera(renderer.H, renderer.W, seed=cam_seed, C_sigma=cond.thresh_sigma,
|
|
102
|
+
noise_rate_hz=cond.ev_noise_rate, drop_prob=cond.drop_prob,
|
|
103
|
+
tau=cond.photoreceptor_tau)
|
|
104
|
+
aps_rng = np.random.default_rng(10_000 + (cam_seed if aps_seed is None else aps_seed))
|
|
105
|
+
aps_exp = cond.frame_exposure()
|
|
106
|
+
aps_hist = deque(maxlen=max(1, int(round(aps_exp / (DT / SUB)))))
|
|
107
|
+
enc = TimeSurfaceEncoder(renderer.H, renderer.W)
|
|
108
|
+
s = np.array(s0, dtype=np.float64)
|
|
109
|
+
cam.reset(renderer.render(s, p.l, 0.0))
|
|
110
|
+
kicks = dict(kicks)
|
|
111
|
+
if hasattr(pipeline, "set_conditions"):
|
|
112
|
+
pipeline.set_conditions(cond)
|
|
113
|
+
if getattr(pipeline, "needs_true_plant", False): # sensor simulation needs the real plant from tick 0
|
|
114
|
+
pipeline.p_true = p
|
|
115
|
+
pipeline.reset(s)
|
|
116
|
+
log = dict(s=[], s_hat=[], u=[], surf=[], events=[], fail_tick=None)
|
|
117
|
+
aps = renderer.render(s, p.l, 0.0).astype(np.float32)
|
|
118
|
+
if not nominal:
|
|
119
|
+
aps = cond.frame_sensor(aps).astype(np.float32)
|
|
120
|
+
aps_pending, next_aps = deque(), 0.0
|
|
121
|
+
t = 0.0
|
|
122
|
+
u = 0.0
|
|
123
|
+
for k in range(n_ticks):
|
|
124
|
+
if snap is not None and k == snap[0]:
|
|
125
|
+
p = snap[1]
|
|
126
|
+
if k in kicks:
|
|
127
|
+
s[3] += kicks[k]
|
|
128
|
+
est = pipeline.estimate()
|
|
129
|
+
u = controller.act(est)
|
|
130
|
+
if exploration is not None:
|
|
131
|
+
u = float(np.clip(u + exploration(), -p.u_max, p.u_max))
|
|
132
|
+
n_ev = 0
|
|
133
|
+
for j in range(SUB):
|
|
134
|
+
s = rk4_step(s, u, DT / SUB, p)
|
|
135
|
+
img = renderer.render(s, p.l, t + (j + 1) * DT / SUB)
|
|
136
|
+
lp, ln, c = cam.step(img, t + j * DT / SUB, DT / SUB)
|
|
137
|
+
enc.update(lp, ln)
|
|
138
|
+
n_ev += c
|
|
139
|
+
if not nominal:
|
|
140
|
+
aps_hist.append(img)
|
|
141
|
+
if aps_fps and t + j * DT / SUB >= next_aps - 1e-9:
|
|
142
|
+
if nominal:
|
|
143
|
+
frame = img.astype(np.float32)
|
|
144
|
+
else: # same sensor model as the frame camera: exposure blur, saturation, noise
|
|
145
|
+
frame = cond.frame_sensor(np.mean(aps_hist, 0))
|
|
146
|
+
frame = (frame + aps_rng.normal(0, cond.frame_noise(aps_exp), frame.shape)).astype(np.float32)
|
|
147
|
+
aps_pending.append((next_aps + aps_latency, frame))
|
|
148
|
+
next_aps += 1.0 / aps_fps
|
|
149
|
+
t += DT
|
|
150
|
+
while aps_pending and aps_pending[0][0] <= t + 1e-9:
|
|
151
|
+
aps = aps_pending.popleft()[1]
|
|
152
|
+
surface = np.concatenate([enc.tensor(t), aps[None]])
|
|
153
|
+
if getattr(pipeline, "needs_true_plant", False): # proprioceptive sensors feel the real plant
|
|
154
|
+
pipeline.p_true = p
|
|
155
|
+
pipeline.update(k, t, s, surface, u)
|
|
156
|
+
log["s"].append(s.copy()); log["u"].append(u); log["events"].append(n_ev)
|
|
157
|
+
log["s_hat"].append(np.array(pipeline.estimate()))
|
|
158
|
+
if record:
|
|
159
|
+
log["surf"].append(surface.astype(np.float16))
|
|
160
|
+
if failed(s, p):
|
|
161
|
+
log["fail_tick"] = k
|
|
162
|
+
if stop_on_fail:
|
|
163
|
+
break
|
|
164
|
+
for key in ("s", "s_hat", "u", "events"):
|
|
165
|
+
log[key] = np.array(log[key])
|
|
166
|
+
if record:
|
|
167
|
+
log["surf"] = np.stack(log["surf"])
|
|
168
|
+
return log
|
aurasync/v4/__init__.py
ADDED
|
File without changes
|
|
@@ -0,0 +1,61 @@
|
|
|
1
|
+
"""Baseline D: independent event-geometry estimator (no neural network).
|
|
2
|
+
|
|
3
|
+
Physics prediction every tick (same nominal RK4 model and process noise as the
|
|
4
|
+
V3 filter) + gated Kalman fusion of the event-geometry measurement (theta and
|
|
5
|
+
pivot x). Velocities are inferred by the filter through the dynamics. When no
|
|
6
|
+
valid measurement exists (quiet scene) it coasts on physics with growing
|
|
7
|
+
covariance. Same pipeline interface as every other estimator.
|
|
8
|
+
"""
|
|
9
|
+
import numpy as np
|
|
10
|
+
|
|
11
|
+
from ..physics import PlantParams, rk4_step, linearise
|
|
12
|
+
from ..estimator import SCALE
|
|
13
|
+
from .geometry import EventGeometry
|
|
14
|
+
|
|
15
|
+
DT = 1e-3
|
|
16
|
+
SCALE_NP = SCALE.numpy().astype(np.float64)
|
|
17
|
+
|
|
18
|
+
|
|
19
|
+
class GeoPipeline:
|
|
20
|
+
def __init__(self, Q=None, gate=9.0, sigma_x_floor=0.004, plant=None, seed=0, name="event-geometry"):
|
|
21
|
+
self.p = plant or PlantParams()
|
|
22
|
+
self.A, _ = linearise(self.p, DT)
|
|
23
|
+
self.Q = np.diag([1e-9, 1e-5, 1e-8, 1e-4]) * 1e4 if Q is None else Q
|
|
24
|
+
self.gate, self.sx = gate, sigma_x_floor
|
|
25
|
+
self.geo = EventGeometry(seed=seed + 1)
|
|
26
|
+
self.seed, self.name = seed, name
|
|
27
|
+
self.delay_ticks = 0
|
|
28
|
+
|
|
29
|
+
def reset(self, s0):
|
|
30
|
+
self.geo.rng = np.random.default_rng(self.seed + 1)
|
|
31
|
+
self.s = np.asarray(s0, float).copy()
|
|
32
|
+
self.Sigma = np.diag((0.05 * SCALE_NP) ** 2)
|
|
33
|
+
self.n_fused = 0
|
|
34
|
+
|
|
35
|
+
def update(self, k, t, s_true, surface, u):
|
|
36
|
+
self.s = rk4_step(self.s, u, DT, self.p)
|
|
37
|
+
self.Sigma = self.A @ self.Sigma @ self.A.T + self.Q
|
|
38
|
+
m = self.geo.measure(surface, self.s[0])
|
|
39
|
+
if m is None or not m["valid"]:
|
|
40
|
+
return
|
|
41
|
+
H = np.array([[0, 0, 1.0, 0], [1.0, 0, 0, 0]])
|
|
42
|
+
z = np.array([m["theta"], m["x_pivot"]])
|
|
43
|
+
R = np.diag([m["sigma_theta"] ** 2, max(self.sx, 0.5 / self.geo.S / np.sqrt(m["n_inliers"])) ** 2])
|
|
44
|
+
S = H @ self.Sigma @ H.T + R
|
|
45
|
+
nu = z - H @ self.s
|
|
46
|
+
if nu[0] ** 2 / S[0, 0] > self.gate and self.n_fused > 50: # outlier rejection once tracking
|
|
47
|
+
return
|
|
48
|
+
K = np.linalg.solve(S, H @ self.Sigma).T
|
|
49
|
+
self.s = self.s + K @ nu
|
|
50
|
+
I_KH = np.eye(4) - K @ H
|
|
51
|
+
self.Sigma = I_KH @ self.Sigma @ I_KH.T + K @ R @ K.T
|
|
52
|
+
self.n_fused += 1
|
|
53
|
+
|
|
54
|
+
def estimate(self):
|
|
55
|
+
return self.s
|
|
56
|
+
|
|
57
|
+
def covariance(self):
|
|
58
|
+
return self.Sigma
|
|
59
|
+
|
|
60
|
+
def age_ticks(self):
|
|
61
|
+
return 0
|
aurasync/v4/geometry.py
ADDED
|
@@ -0,0 +1,97 @@
|
|
|
1
|
+
"""Independent event-derived geometric measurement of the pole (V4, Modules A + B).
|
|
2
|
+
|
|
3
|
+
Analogy: instead of asking the navigator where the ship is, look out of the window
|
|
4
|
+
and line up the mast against the horizon. No memory, no learned model: just the
|
|
5
|
+
geometry of where events are firing right now.
|
|
6
|
+
|
|
7
|
+
The pole is a straight segment from the pivot (x_p, PIVOT_Y) at angle theta.
|
|
8
|
+
A moving pole fires events along its edges, so the fresh events (younger than
|
|
9
|
+
`window` seconds; their age is exactly recoverable from the time surface) lie
|
|
10
|
+
near the line
|
|
11
|
+
x = x_p + (y - PIVOT_Y) * tan(theta)
|
|
12
|
+
Fitting x as a function of y is well conditioned for the angles that matter
|
|
13
|
+
(|theta| < 60 deg). The fit is robust (RANSAC + least-squares refinement on
|
|
14
|
+
inliers) because background noise events are scattered uniformly.
|
|
15
|
+
|
|
16
|
+
Output per call (or None when the evidence is insufficient):
|
|
17
|
+
theta, x_pivot measured pole angle and pivot position
|
|
18
|
+
sigma_theta measurement standard deviation (from inlier scatter,
|
|
19
|
+
inlier count and vertical extent, floored)
|
|
20
|
+
n_inliers, span support of the fit (span = fraction of pole length covered)
|
|
21
|
+
snr inlier density / background event density
|
|
22
|
+
quality in [0, 1] combined reliability used for gating
|
|
23
|
+
Uses nothing but the sensor surface and the nominal pole length (no ground truth).
|
|
24
|
+
"""
|
|
25
|
+
import numpy as np
|
|
26
|
+
|
|
27
|
+
from ..event_camera import SceneRenderer
|
|
28
|
+
|
|
29
|
+
PIVOT_Y = 0.04
|
|
30
|
+
TAU_FAST = 0.008
|
|
31
|
+
|
|
32
|
+
|
|
33
|
+
class EventGeometry:
|
|
34
|
+
def __init__(self, l=0.25, window=0.003, band_halfwidth=0.6, inlier_px=1.5, n_iter=60,
|
|
35
|
+
min_inliers=8, min_span=0.35, min_snr=4.0, sigma_floor_deg=0.4, seed=0):
|
|
36
|
+
r = SceneRenderer()
|
|
37
|
+
self.S = r.S
|
|
38
|
+
self.wx, self.wy = r.wx, r.wy
|
|
39
|
+
self.l, self.band = l, band_halfwidth
|
|
40
|
+
self.fresh = float(np.exp(-window / TAU_FAST))
|
|
41
|
+
self.inl = inlier_px / r.S
|
|
42
|
+
self.n_iter, self.min_inliers, self.min_span, self.min_snr = n_iter, min_inliers, min_span, min_snr
|
|
43
|
+
self.sigma_floor = np.radians(sigma_floor_deg)
|
|
44
|
+
self.rng = np.random.default_rng(seed)
|
|
45
|
+
self.y_lo, self.y_hi = PIVOT_Y + 0.06, PIVOT_Y + 2 * l + 0.04 # above the cart, up to the tip
|
|
46
|
+
|
|
47
|
+
def events(self, surface):
|
|
48
|
+
ev = (surface[0] >= self.fresh) | (surface[1] >= self.fresh)
|
|
49
|
+
return ev
|
|
50
|
+
|
|
51
|
+
def measure(self, surface, x_hint):
|
|
52
|
+
"""Fit the pole line to fresh events in a vertical band around x_hint (the current
|
|
53
|
+
pivot estimate; only used to choose where to look)."""
|
|
54
|
+
ev = self.events(surface)
|
|
55
|
+
roi = (self.wy >= self.y_lo) & (self.wy <= self.y_hi) & (np.abs(self.wx - x_hint) <= self.band)
|
|
56
|
+
sel = ev & roi
|
|
57
|
+
n = int(sel.sum())
|
|
58
|
+
if n < self.min_inliers:
|
|
59
|
+
return None
|
|
60
|
+
X, Y = self.wx[sel], self.wy[sel] - PIVOT_Y
|
|
61
|
+
# background density from fresh events outside the region (per pixel)
|
|
62
|
+
out = ev & ~roi & (self.wy > self.y_lo)
|
|
63
|
+
bg_density = out.sum() / max((~roi & (self.wy > self.y_lo)).sum(), 1)
|
|
64
|
+
# vectorised RANSAC: all hypotheses scored in one array operation
|
|
65
|
+
i = self.rng.integers(0, n, self.n_iter); j = self.rng.integers(0, n, self.n_iter)
|
|
66
|
+
dy = Y[i] - Y[j]
|
|
67
|
+
ok = np.abs(dy) >= 0.08
|
|
68
|
+
if not ok.any():
|
|
69
|
+
return None
|
|
70
|
+
i, j, dy = i[ok], j[ok], dy[ok]
|
|
71
|
+
slope = (X[i] - X[j]) / dy # tan(theta) per hypothesis
|
|
72
|
+
keep = np.abs(slope) <= np.tan(np.radians(70))
|
|
73
|
+
if not keep.any():
|
|
74
|
+
return None
|
|
75
|
+
slope, x0 = slope[keep], (X[i] - slope * Y[i])[keep]
|
|
76
|
+
inl_all = np.abs(X[None, :] - (x0[:, None] + slope[:, None] * Y[None, :])) <= self.inl
|
|
77
|
+
b = int(inl_all.sum(1).argmax())
|
|
78
|
+
best_in = inl_all[b]
|
|
79
|
+
if best_in.sum() < self.min_inliers:
|
|
80
|
+
return None
|
|
81
|
+
Xi, Yi = X[best_in], Y[best_in]
|
|
82
|
+
A = np.stack([np.ones_like(Yi), Yi], 1)
|
|
83
|
+
(x0, slope), *_ = np.linalg.lstsq(A, Xi, rcond=None) # refine on inliers
|
|
84
|
+
res = Xi - (x0 + slope * Yi)
|
|
85
|
+
theta = float(np.arctan(slope))
|
|
86
|
+
span = float((Yi.max() - Yi.min()) / (2 * self.l))
|
|
87
|
+
# inlier band area (pixels) for the noise comparison
|
|
88
|
+
band_px = 2 * self.inl * self.S * (Yi.max() - Yi.min()) * self.S + 1e-9
|
|
89
|
+
snr = float(best_in.sum() / max(bg_density * band_px, 0.5))
|
|
90
|
+
# angle uncertainty: horizontal scatter over the vertical lever arm (Gauss-Markov), floored
|
|
91
|
+
sx = max(float(res.std()), 0.5 / self.S)
|
|
92
|
+
sy = max(float(Yi.std()), 1e-3)
|
|
93
|
+
sigma = float(np.hypot(sx / (sy * np.sqrt(best_in.sum())), self.sigma_floor)) / np.cos(theta) ** 2
|
|
94
|
+
quality = float(np.clip(min(span / 0.8, snr / 10.0, best_in.sum() / 30.0), 0, 1))
|
|
95
|
+
valid = span >= self.min_span and snr >= self.min_snr
|
|
96
|
+
return dict(theta=theta, x_pivot=float(x0), sigma_theta=sigma, n_inliers=int(best_in.sum()),
|
|
97
|
+
n_events=n, span=span, snr=snr, quality=quality, valid=bool(valid))
|