diff --git a/retina_tracker/cli.py b/retina_tracker/cli.py index 8680e1b..dfb0b2a 100644 --- a/retina_tracker/cli.py +++ b/retina_tracker/cli.py @@ -14,7 +14,7 @@ load_config, set_config, ) -from .output import TrackEventWriter +from .output import InnovationWriter, TrackEventWriter from .server import run_tcp_server from .tracker import UNBOUNDED_ARCHIVE, Tracker @@ -47,7 +47,7 @@ def load_detections(filepath): return [] -def process_detections(detections_file, event_writer=None, detection_window=20): +def process_detections(detections_file, event_writer=None, detection_window=20, innovation_writer=None): """Process all detections and generate tracks.""" output = sys.stderr if event_writer and event_writer._is_stdout else sys.stdout @@ -60,6 +60,7 @@ def process_detections(detections_file, event_writer=None, detection_window=20): detection_window=detection_window, config=get_config(), max_completed_tracks=UNBOUNDED_ARCHIVE, + innovation_writer=innovation_writer, ) for i, frame in enumerate(detection_frames): @@ -199,6 +200,13 @@ def main(): default=20, help="Number of detections to include in sliding window (default: 20)", ) + parser.add_argument( + "--innovations", + type=str, + help="Output file for per-update innovation records, for calibrating R and Q. " + "Off unless given: the events file records what was associated, not what " + "the filter predicted beforehand, and only the difference constrains either.", + ) parser.add_argument("-c", "--config", type=str, help="Path to configuration file (default: config.yaml)") parser.add_argument("--blah2-config", type=str, help="Path to blah2 config.yml to read center frequency (fc)") @@ -256,6 +264,14 @@ def main(): elif args.tcp: event_writer = TrackEventWriter("-") + innovation_writer = None + if args.innovations: + innovation_writer = InnovationWriter( + args.innovations, + max_bytes=OUTPUT_MAX_BYTES(), + backup_count=OUTPUT_BACKUP_COUNT(), + ) + if args.tcp: run_tcp_server( host=args.tcp_host, @@ -267,12 +283,20 @@ def main(): control_port=args.control_port, history_window_s=args.history_window, history_max_points=args.history_max_points, + innovation_writer=innovation_writer, ) else: - tracker = process_detections(args.file, event_writer=event_writer, detection_window=args.detection_window) + tracker = process_detections( + args.file, + event_writer=event_writer, + detection_window=args.detection_window, + innovation_writer=innovation_writer, + ) if event_writer: event_writer.close() + if innovation_writer: + innovation_writer.close() save_tracks(tracker, args.output) diff --git a/retina_tracker/kalman.py b/retina_tracker/kalman.py index 0f8b904..f0be006 100644 --- a/retina_tracker/kalman.py +++ b/retina_tracker/kalman.py @@ -1,6 +1,7 @@ """Kalman filter over bistatic range for delay-Doppler detections.""" import sys +from typing import NamedTuple import numpy as np @@ -33,6 +34,27 @@ def range_rate_to_doppler(range_rate_km_s): return -range_rate_km_s / WAVELENGTH_KM() +class Residual(NamedTuple): + """What one update revealed about the filter's own consistency. + + The innovation and its covariance are what R and Q are answerable to: R is + the floor of S, and Q sets how fast the state's share of S grows between + updates. Returning them rather than the NIS alone is what lets the two be + separated from recorded data, which a single scalar cannot do because a + too-large R and a too-small Q move it in opposite directions and cancel. + """ + + innovation: np.ndarray + S: np.ndarray + nis: float + + @classmethod + def degenerate(cls): + """A singular S skipped the measurement, so nothing was learned.""" + nan = np.full(MEASUREMENT_DIM, np.nan) + return cls(nan, np.full((MEASUREMENT_DIM, MEASUREMENT_DIM), np.nan), float(MEASUREMENT_DIM)) + + class KalmanFilter: """Constant-acceleration filter on bistatic range. @@ -128,13 +150,13 @@ def update(self, state, covariance, measurement, snr=None): K = covariance @ self.H.T @ np.linalg.inv(S) except np.linalg.LinAlgError: print("Warning: Singular innovation covariance in Kalman update, skipping measurement", file=sys.stderr) - return state, covariance, float(MEASUREMENT_DIM) + return state, covariance, Residual.degenerate() state_upd = state + K @ innovation cov_upd = (np.eye(self.dim_state) - K @ self.H) @ covariance nis = float(innovation @ np.linalg.solve(S, innovation)) - return state_upd, _symmetrised(cov_upd), nis + return state_upd, _symmetrised(cov_upd), Residual(innovation, S, nis) def get_innovation_covariance(self, covariance, snr=None): S = self.H @ covariance @ self.H.T + self.R * self.measurement_noise_scale(snr) diff --git a/retina_tracker/output.py b/retina_tracker/output.py index d99f508..b844657 100644 --- a/retina_tracker/output.py +++ b/retina_tracker/output.py @@ -104,8 +104,9 @@ def write_event( "anomaly_types": sorted(anomaly_types) if anomaly_types else [], "shadow_fraction": shadow_fraction, } - line = json.dumps(event) + "\n" + self._write_line(json.dumps(event) + "\n") + def _write_line(self, line): if not self._is_stdout and self.max_bytes: size = len(line.encode("utf-8")) if self.bytes_written and self.bytes_written + size > self.max_bytes: @@ -129,3 +130,46 @@ def _rotate(self): def close(self): if not self._is_stdout: self.output.close() + + +class InnovationWriter(TrackEventWriter): + """Writes one record per Kalman update, for calibrating R and Q. + + Off unless a path is given. R and Q cannot be fitted from the events file: + it carries the detections that were associated, not what the filter + predicted before seeing them, and the difference between those is the only + quantity either constant answers to. + + Reconstructing innovations offline by replaying recorded detections works + but cannot see the filter's real state, because a replay has to guess at + the covariance, the adaptive process-noise scale and the coasting history + that shaped each prediction. This records them instead of inferring them. + + Inherits the size bound so a node left recording cannot fill its disk. + """ + + def write_residual(self, track_id, timestamp, track, detection): + residual = track.last_residual + if residual is None: + return + + record = { + "track_id": track_id, + "birth": track.birth_timestamp, + "timestamp": timestamp, + "dt": track.last_dt, + "snr": detection.get("snr"), + # The measurement, so a record can be filtered on its own. Most + # tracks at an interfered site are built on a fixed-Doppler tone + # and must be excluded before anything is fitted to them; without + # the Doppler here that can only be done by joining back to the + # events file, which a track that never confirmed is absent from. + "delay": detection.get("delay"), + "doppler": detection.get("doppler"), + "n_missed": track.last_n_missed, + "q_scale": track.last_q_scale, + "innovation": [float(x) for x in residual.innovation], + "s_diag": [float(residual.S[0][0]), float(residual.S[1][1])], + "nis": residual.nis, + } + self._write_line(json.dumps(record) + "\n") diff --git a/retina_tracker/server.py b/retina_tracker/server.py index 45a80e3..b22ace4 100644 --- a/retina_tracker/server.py +++ b/retina_tracker/server.py @@ -170,6 +170,7 @@ def run_tcp_server( control_port=CONTROL_PORT, history_window_s=WINDOW_S, history_max_points=MAX_POINTS, + innovation_writer=None, ): """Run tracker as TCP server receiving detection frames from blah2. @@ -183,6 +184,7 @@ def run_tcp_server( control_port: Port for the HTTP control surface; 0 disables it history_window_s: How much of the recent past to keep in memory history_max_points: Hard ceiling per detection class, whatever the rate + innovation_writer: InnovationWriter for R/Q calibration, or None """ # In memory rather than on disk: it does not need to survive a restart, # and the alternative was several hundred megabytes a day onto an SD card. @@ -195,6 +197,7 @@ def run_tcp_server( detection_window=detection_window, config=config or get_config(), detection_sink=history, + innovation_writer=innovation_writer, ) start_pruner(history) diff --git a/retina_tracker/track.py b/retina_tracker/track.py index 283a50c..600cfda 100644 --- a/retina_tracker/track.py +++ b/retina_tracker/track.py @@ -117,6 +117,10 @@ def __init__(self, detection, timestamp, kf, frame=0, config=None): self.n_missed = 0 self.n_shadow_obs = 1 self.nis_ema = float(MEASUREMENT_DIM) + self.last_dt = None + self.last_residual = None + self.last_q_scale = 1.0 + self.last_n_missed = 0 self.n_shadowed = 1 if detection.get("shadowed") else 0 self.total_snr = detection["snr"] @@ -711,6 +715,7 @@ def process_noise_scale(self): def predict(self, dt): self.kf.dt = dt + self.last_dt = dt state_pred, cov_pred = self.kf.predict(self.state, self.covariance, self.process_noise_scale()) self.state = state_pred # Freeze covariance growth once coasting exceeds N_COAST so the @@ -721,11 +726,19 @@ def predict(self, dt): def update(self, detection, timestamp, frame=0): measurement = np.array([detection["delay"], doppler_to_range_rate(detection["doppler"])]) - self.state, self.covariance, nis = self.kf.update( + q_scale = self.process_noise_scale() + self.state, self.covariance, residual = self.kf.update( self.state, self.covariance, measurement, detection.get("snr") ) memory = PROCESS_NOISE_NIS_MEMORY() - self.nis_ema = (1.0 - memory) * self.nis_ema + memory * nis + self.nis_ema = (1.0 - memory) * self.nis_ema + memory * residual.nis + self.last_residual = residual + self.last_q_scale = q_scale + # Before the reset below, which is what makes this worth latching: how + # long the track had been coasting is a property of the prediction this + # innovation was measured against, and by the end of this method it is + # gone. + self.last_n_missed = self.n_missed # Identity swap check MUST run before adsb_hex capture self._check_identity_change_anomaly(detection, timestamp) diff --git a/retina_tracker/tracker.py b/retina_tracker/tracker.py index ba04507..0edb261 100644 --- a/retina_tracker/tracker.py +++ b/retina_tracker/tracker.py @@ -49,6 +49,7 @@ def __init__( max_completed_tracks=MAX_COMPLETED_TRACKS, detection_sink=None, max_pending_classification_frames=MAX_PENDING_CLASSIFICATION_FRAMES, + innovation_writer=None, ): self.kf = KalmanFilter() self.tracks = [] @@ -63,6 +64,10 @@ def __init__( # (see _drain_classifications). None costs nothing: the bookkeeping is # skipped entirely rather than computed and dropped. self.detection_sink = detection_sink + # Optional. Receives one record per Kalman update so R and Q can be + # calibrated from what the filter actually predicted. None costs + # nothing: no record is built rather than built and dropped. + self.innovation_writer = innovation_writer self._max_pending_frames = max_pending_classification_frames self._pending_classification = deque() @@ -170,6 +175,8 @@ def process_frame(self, detections, timestamp): track = self.tracks[track_idx] det = detections[det_idx] track.update(det, timestamp, frame=self.frame_count) + if self.innovation_writer: + self.innovation_writer.write_residual(track.id, timestamp, track, det) if track.state_status == TrackState.COASTING: track.state_status = TrackState.ACTIVE associated_tracks.add(track_idx) diff --git a/tests/test_adaptive_process_noise.py b/tests/test_adaptive_process_noise.py index c294c25..d3f8689 100644 --- a/tests/test_adaptive_process_noise.py +++ b/tests/test_adaptive_process_noise.py @@ -71,8 +71,8 @@ def test_update_returns_the_normalised_innovation(self): kf = KalmanFilter() state = np.array([10.0, 0.0, 0.0]) cov = np.diag([1.0, 1e-5, 1e-4]) - _, _, nis = kf.update(state, cov, np.array([10.0, 0.0]), snr=15.0) - assert nis == pytest.approx(0.0, abs=1e-9) + _, _, residual = kf.update(state, cov, np.array([10.0, 0.0]), snr=15.0) + assert residual.nis == pytest.approx(0.0, abs=1e-9) def test_a_large_innovation_gives_a_large_nis(self): kf = KalmanFilter() @@ -80,7 +80,7 @@ def test_a_large_innovation_gives_a_large_nis(self): cov = np.diag([1.0, 1e-5, 1e-4]) _, _, near = kf.update(state, cov, np.array([10.1, 0.0]), snr=15.0) _, _, far = kf.update(state, cov, np.array([14.0, 0.0]), snr=15.0) - assert far > near + assert far.nis > near.nis def test_a_wider_scale_grows_the_covariance_faster(self): kf = KalmanFilter(dt=1.0) diff --git a/tests/test_innovations.py b/tests/test_innovations.py new file mode 100644 index 0000000..cd7780a --- /dev/null +++ b/tests/test_innovations.py @@ -0,0 +1,222 @@ +"""Per-update innovation records, which are what R and Q are answerable to. + +The events file records the detections that were associated, never what the +filter predicted before it saw them. R is the floor of the innovation +covariance and Q sets how fast the state's share of it grows between updates, +so neither can be fitted from events alone. These records close that gap. +""" + +import json + +import numpy as np +import pytest + +from retina_tracker.kalman import KalmanFilter, Residual, doppler_to_range_rate +from retina_tracker.output import InnovationWriter +from retina_tracker.tracker import Tracker + +BASE_TS = 1718747745000 + + +def frame(delay, doppler=-120.0, snr=16.0): + return [{"delay": delay, "doppler": doppler, "snr": snr}] + + +def run(tracker, n=8, step_ms=500, delay0=20.0, drift=-0.05): + for i in range(n): + tracker.process_frame(frame(delay0 + i * drift), BASE_TS + i * step_ms) + + +def read(path): + with open(path) as handle: + return [json.loads(line) for line in handle if line.strip()] + + +class TestTheFilterReportsItsOwnResidual: + def test_update_returns_the_innovation_not_just_its_norm(self): + kf = KalmanFilter() + state = np.array([20.0, -0.1, 0.0]) + cov = np.eye(3) + + _, _, residual = kf.update(state, cov, np.array([20.5, -0.1]), 16.0) + + assert residual.innovation[0] == pytest.approx(0.5) + assert residual.S.shape == (2, 2) + + def test_nis_is_still_the_quadratic_form_it_always_was(self): + kf = KalmanFilter() + state = np.array([20.0, -0.1, 0.0]) + cov = np.eye(3) + measurement = np.array([20.5, -0.1]) + + _, _, residual = kf.update(state, cov, measurement, 16.0) + + expected = float(residual.innovation @ np.linalg.solve(residual.S, residual.innovation)) + assert residual.nis == pytest.approx(expected) + + def test_a_singular_covariance_reports_nothing_learned(self): + """The measurement was skipped, so the record must not claim a residual.""" + residual = Residual.degenerate() + + assert np.isnan(residual.innovation).all() + assert np.isnan(residual.S).all() + + +class TestRecordsReachTheFile: + def test_one_record_per_update(self, tmp_path): + path = tmp_path / "innovations.jsonl" + writer = InnovationWriter(str(path), max_bytes=0) + tracker = Tracker(innovation_writer=writer) + + run(tracker) + writer.close() + + records = read(path) + assert records + assert all(r["innovation"] and len(r["innovation"]) == 2 for r in records) + + def test_a_record_carries_what_calibration_needs(self, tmp_path): + """dt and q_scale separate R from Q; without them the two cancel.""" + path = tmp_path / "innovations.jsonl" + writer = InnovationWriter(str(path), max_bytes=0) + tracker = Tracker(innovation_writer=writer) + + run(tracker) + writer.close() + + record = read(path)[-1] + assert set(record) == { + "track_id", + "birth", + "timestamp", + "dt", + "snr", + "delay", + "doppler", + "n_missed", + "q_scale", + "innovation", + "s_diag", + "nis", + } + assert record["dt"] == pytest.approx(0.5) + assert record["snr"] == pytest.approx(16.0) + assert record["s_diag"][0] > 0 + + def test_a_record_can_be_filtered_without_the_events_file(self, tmp_path): + """Most tracks at an interfered site sit on a fixed-Doppler tone and + have to be dropped before anything is fitted. A track that never + confirmed has no id and never reaches the events file, so the Doppler + has to travel with the record itself.""" + path = tmp_path / "innovations.jsonl" + writer = InnovationWriter(str(path), max_bytes=0) + tracker = Tracker(innovation_writer=writer) + + run(tracker, n=5) + writer.close() + + records = read(path) + assert all(r["doppler"] is not None for r in records) + assert all(r["birth"] == records[0]["birth"] for r in records) + + def test_the_recorded_innovation_matches_the_measurement_it_came_from(self, tmp_path): + path = tmp_path / "innovations.jsonl" + writer = InnovationWriter(str(path), max_bytes=0) + tracker = Tracker(innovation_writer=writer) + + run(tracker, n=3) + writer.close() + + for record in read(path): + assert abs(record["innovation"][0]) < 5.0 + assert record["nis"] >= 0.0 + + def test_q_scale_is_the_one_in_force_for_that_prediction(self, tmp_path): + """Recorded after the update it would be the next frame's scale, which + is not the value that shaped the covariance this innovation was + measured against.""" + path = tmp_path / "innovations.jsonl" + writer = InnovationWriter(str(path), max_bytes=0) + tracker = Tracker(innovation_writer=writer) + + run(tracker, n=6) + writer.close() + + assert all(r["q_scale"] >= 1.0 for r in read(path)) + + +class TestTheCoastingHistoryIsNotLost: + """How long a track had been coasting is the one input to the prediction + that a replay cannot reconstruct, and it is the reason these records exist + rather than an offline reconstruction. Track.update() zeroes n_missed + before returning, so reading it from the track afterwards gives 0 every + time and the longest predictions - the ones Q answers to - look like the + shortest.""" + + def _coasted(self, tmp_path, gap): + path = tmp_path / "innovations.jsonl" + writer = InnovationWriter(str(path), max_bytes=0) + tracker = Tracker(innovation_writer=writer) + + run(tracker, n=6) + for i in range(gap): + tracker.process_frame([], BASE_TS + (6 + i) * 500) + tracker.process_frame(frame(20.0 - (6 + gap) * 0.05), BASE_TS + (6 + gap) * 500) + writer.close() + return read(path) + + def test_a_reassociation_records_the_frames_it_coasted(self, tmp_path): + assert self._coasted(tmp_path, gap=2)[-1]["n_missed"] == 2 + + def test_an_uninterrupted_update_still_records_none(self, tmp_path): + assert self._coasted(tmp_path, gap=0)[-1]["n_missed"] == 0 + + +class TestOffByDefault: + def test_no_writer_means_no_records_and_no_cost(self, tmp_path): + tracker = Tracker() + + run(tracker) + + assert tracker.innovation_writer is None + assert not list(tmp_path.iterdir()) + + def test_tracking_is_unchanged_by_recording(self, tmp_path): + path = tmp_path / "innovations.jsonl" + writer = InnovationWriter(str(path), max_bytes=0) + recorded = Tracker(innovation_writer=writer) + plain = Tracker() + + run(recorded) + run(plain) + writer.close() + + assert len(recorded.tracks) == len(plain.tracks) + assert [t.state.tolist() for t in recorded.tracks] == [t.state.tolist() for t in plain.tracks] + + +class TestTheFileStaysBounded: + def test_recording_cannot_fill_a_node_disk(self, tmp_path): + """A node left recording writes one record per update indefinitely.""" + path = tmp_path / "innovations.jsonl" + writer = InnovationWriter(str(path), max_bytes=400, backup_count=1) + tracker = Tracker(innovation_writer=writer) + + run(tracker, n=40) + writer.close() + + assert path.stat().st_size <= 400 + assert (tmp_path / "innovations.jsonl.1").exists() + + +class TestMeasurementConversion: + def test_the_rate_innovation_is_in_range_rate_not_hertz(self): + """S and R live in km/s, so an innovation in Hz would be inconsistent + with the covariance it is divided by.""" + kf = KalmanFilter() + state = np.array([20.0, doppler_to_range_rate(-120.0), 0.0]) + cov = np.eye(3) + + _, _, residual = kf.update(state, cov, np.array([20.0, doppler_to_range_rate(-130.0)]), 16.0) + + assert residual.innovation[1] == pytest.approx(doppler_to_range_rate(-130.0) - doppler_to_range_rate(-120.0))