267 lines
12 KiB
Python
267 lines
12 KiB
Python
"""Radar sample geometry and forecast drawing."""
|
|
import time
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
from defense_sim.entities.projectile import Projectile, ProjectileConfig
|
|
from defense_sim.guidance.activation import CityCircle
|
|
from defense_sim.guidance.trajectory import regress_track
|
|
from defense_sim.sensors.devices import Aeris10Radar, AerisRadarConfig
|
|
from defense_sim.sensors.models import Observation
|
|
from defense_sim.sensors.signal_chain import RANGE_BIN_M, ReceiverProcessor, RadarSignalConfig
|
|
from defense_sim.simulation.engine import SimulationEngine
|
|
|
|
|
|
def _projectile(position, velocity):
|
|
return Projectile("p", np.array(position, dtype=float), np.array(velocity, dtype=float),
|
|
ProjectileConfig(area_m2=2.0, mass_kg=300.0))
|
|
|
|
|
|
def test_operational_range_stays_on_the_target_not_the_max_range_arc():
|
|
radar = Aeris10Radar("r", np.zeros(3), 0.0, AerisRadarConfig(), np.random.default_rng(1))
|
|
target = _projectile([12000., 0., -2000.], [-140., 0., 5.])
|
|
truth_range = float(np.linalg.norm(target.position_ned_m))
|
|
measured = radar._process_at_dwell(target, 1.0, 0.0)
|
|
assert measured is not None
|
|
assert measured["range_m"] == pytest.approx(truth_range, abs=RANGE_BIN_M)
|
|
assert measured["range_m"] < 15_000
|
|
assert np.linalg.norm(measured["position_ned_m"] - target.position_ned_m) < 500
|
|
|
|
|
|
def test_second_dwell_range_rate_is_unambiguous_and_approaching_negative():
|
|
radar = Aeris10Radar("r", np.zeros(3), 0.0, AerisRadarConfig(), np.random.default_rng(2))
|
|
target = _projectile([14000., 0., -1500.], [-120., 0., 0.])
|
|
first = radar._process_at_dwell(target, 0.0, 0.0)
|
|
target.position_ned_m = np.array([12500., 0., -1500.])
|
|
second = radar._process_at_dwell(target, 12.0, 0.0)
|
|
assert first is not None and second is not None
|
|
# Range fell, so Stone Soup's receding-positive range-rate is negative.
|
|
assert second["radial_velocity_m_s"] < -50
|
|
assert second["doppler_sigma_m_s"] < 10
|
|
|
|
|
|
def test_doppler_unwrap_period_is_one_fft_cycle():
|
|
proc = ReceiverProcessor(np.random.default_rng(0), RadarSignalConfig())
|
|
# 60 m/s is one alias past the ±43 m/s bin; a 2x period maps it to ~144 m/s.
|
|
proc.unwrap_reference_m_s = 60.0
|
|
frame = proc.generate_frame([{"range_m": 2000.0, "radial_velocity_m_s": 60.0, "rcs_dbsm": 10.0}])
|
|
detections = proc.process_frame(*frame)
|
|
assert detections
|
|
best = min(detections, key=lambda row: abs(row["radial_velocity_m_s"] - 60.0))
|
|
assert best["radial_velocity_m_s"] == pytest.approx(60.0, abs=8.0)
|
|
|
|
|
|
def test_descending_forecast_ends_on_the_ground_without_a_time_axis_kink():
|
|
projectile = _projectile([2000., 0., -400.], [-30., 0., 40.])
|
|
sim = SimulationEngine(seed=1, projectiles=[projectile], city_circle=CityCircle(np.zeros(3), 500.))
|
|
sim.radar_tracks["p"] = Observation(
|
|
"p", 0., projectile.position_ned_m.copy(), projectile.velocity_ned_m_s.copy(),
|
|
np.zeros(3), np.zeros(3), np.eye(6), 5, True)
|
|
sim.radar_sample_history["r"] = [{
|
|
"target_id": "p", "measurement_time_s": 0., "position_ned_m": projectile.position_ned_m.tolist()}]
|
|
paths, _details = sim._display_forecasts()
|
|
path = np.asarray(paths["p"])
|
|
assert path.shape[1] == 3
|
|
assert path[-1, 2] == pytest.approx(0.0, abs=1.0)
|
|
assert abs(path[-1, 0] - path[0, 0]) < 5000
|
|
|
|
|
|
def test_level_forecast_stays_straight_and_does_not_drop_to_the_ground():
|
|
projectile = _projectile([8000., 0., -2000.], [-120., 0., 0.])
|
|
sim = SimulationEngine(seed=1, projectiles=[projectile], city_circle=CityCircle(np.zeros(3), 500.))
|
|
sim.radar_tracks["p"] = Observation(
|
|
"p", 0., projectile.position_ned_m.copy(), projectile.velocity_ned_m_s.copy(),
|
|
np.zeros(3), np.zeros(3), np.eye(6), 5, True)
|
|
sim.radar_sample_history["r"] = [{
|
|
"target_id": "p", "measurement_time_s": 0., "position_ned_m": projectile.position_ned_m.tolist()}]
|
|
paths, _details = sim._display_forecasts()
|
|
path = np.asarray(paths["p"])
|
|
delta = path[-1] - path[0]
|
|
assert delta[0] < -100.0
|
|
assert abs(delta[2]) < 1.0
|
|
assert path[:, 2].max() < -1.0
|
|
|
|
|
|
def _line_samples(times, outlier=None):
|
|
rows = []
|
|
for time_s in times:
|
|
rows.append({
|
|
"target_id": "p", "measurement_time_s": time_s,
|
|
"position_ned_m": [1000.0 - 50.0 * time_s, 0.0, -20.0],
|
|
"lateral_sigma_m": 40.0, "vertical_sigma_m": 25.0, "accepted": True,
|
|
})
|
|
if outlier is not None:
|
|
rows.append(outlier)
|
|
return rows
|
|
|
|
|
|
def test_snapshot_altitudes_stay_a_shallow_descent():
|
|
"""Archived run 0da77a10. The first and last returns descend at 5.5 m/s.
|
|
|
|
The points in between scatter by about ±150 m, which is most of that
|
|
descent, so a constant-velocity fit of all seven is shallower. A 2 s
|
|
pair in the same track is about 70 m/s and must not become the line.
|
|
"""
|
|
times = (27.89, 40.80, 53.71, 66.61, 71.52, 77.45, 79.52)
|
|
positions = (
|
|
[31413.6, 69.7, -1555.9],
|
|
[29749.8, 80.9, -1278.6],
|
|
[28094.3, -230.6, -1192.3],
|
|
[26400.3, -250.7, -1288.6],
|
|
[25842.6, -54.2, -1177.2],
|
|
[25159.1, -503.6, -1416.6],
|
|
[24740.8, 0.7, -1271.1],
|
|
)
|
|
rows = [{
|
|
"measurement_time_s": time_s, "position_ned_m": position,
|
|
"lateral_sigma_m": 80.0, "vertical_sigma_m": 120.0, "accepted": True,
|
|
} for time_s, position in zip(times, positions)]
|
|
fit = regress_track(rows, 83.07, tau_s=40.0)
|
|
assert fit["velocity"][0] == pytest.approx(-128.0, abs=8.0)
|
|
assert 0.5 < fit["velocity"][2] < 4.5
|
|
|
|
|
|
def test_weighted_regression_keeps_a_rejected_outlier_off_the_line():
|
|
rows = _line_samples((0.0, 0.7, 1.4, 2.1), {
|
|
"target_id": "p", "measurement_time_s": 2.0,
|
|
"position_ned_m": [5000.0, 8000.0, -20.0],
|
|
"lateral_sigma_m": 80.0, "vertical_sigma_m": 40.0, "accepted": False,
|
|
})
|
|
fit = regress_track(rows, 2.2)
|
|
assert fit["quality"] == "stable"
|
|
assert fit["velocity"][0] == pytest.approx(-50.0, abs=8.0)
|
|
assert abs(fit["velocity"][1]) < 8.0
|
|
assert abs(fit["velocity"][2]) < 5.0
|
|
|
|
|
|
def test_smoothed_forecast_heading_does_not_flip_on_one_bad_dwell():
|
|
projectile = _projectile([890.0, 0.0, -20.0], [-50.0, 0.0, 0.0])
|
|
sim = SimulationEngine(seed=1, projectiles=[projectile], city_circle=CityCircle(np.zeros(3), 500.))
|
|
sim.radar_sample_history["r"] = _line_samples((0.0, 0.7, 1.4, 2.1))
|
|
sim.time_s = 2.2
|
|
first = sim._forecast_state("p")
|
|
again = sim._forecast_state("p")
|
|
assert again is first
|
|
sim.time_s = 2.4
|
|
sim.radar_sample_history["r"].append({
|
|
"target_id": "p", "measurement_time_s": 2.3,
|
|
"position_ned_m": [4000.0, 6000.0, -800.0],
|
|
"lateral_sigma_m": 50.0, "vertical_sigma_m": 30.0, "accepted": True,
|
|
})
|
|
second = sim._forecast_state("p")
|
|
first_v, second_v = first["velocity"], second["velocity"]
|
|
cosine = float(np.dot(first_v, second_v) / (np.linalg.norm(first_v) * np.linalg.norm(second_v)))
|
|
assert cosine > 0.9
|
|
assert abs(second_v[1]) < 15.0
|
|
|
|
|
|
def _track_samples(times, rng, noise):
|
|
rows = []
|
|
for time_s in times:
|
|
position = np.array([8000.0 - 140.0 * time_s, 0.0, -1200.0 + 3.0 * time_s])
|
|
position += rng.normal(0.0, noise)
|
|
rows.append({
|
|
"target_id": "p", "measurement_time_s": float(time_s),
|
|
"position_ned_m": position.tolist(),
|
|
"lateral_sigma_m": float(noise[0]), "vertical_sigma_m": float(noise[2]),
|
|
"accepted": True,
|
|
})
|
|
return rows
|
|
|
|
|
|
def test_radar_projection_eases_when_new_samples_arrive():
|
|
rng = np.random.default_rng(4)
|
|
projectile = _projectile([800.0, 0.0, -1100.0], [-140.0, 0.0, 3.0])
|
|
sim = SimulationEngine(seed=1, projectiles=[projectile], city_circle=CityCircle(np.zeros(3), 500.))
|
|
sim.radar_sample_history["r"] = _track_samples(np.arange(0.0, 8.0, 0.5), rng, (40.0, 40.0, 25.0))
|
|
sim.time_s = 8.0
|
|
settled = sim._forecast_state("p")
|
|
settled_end = settled["position"] + settled["velocity"] * 20.0
|
|
for index, time_s in enumerate(np.arange(8.4, 10.4, 0.4)):
|
|
sim.radar_sample_history["r"].extend(_track_samples([time_s], rng, (180.0, 180.0, 120.0)))
|
|
sim.time_s = float(time_s)
|
|
state = sim._forecast_state("p")
|
|
end = state["position"] + state["velocity"] * 20.0
|
|
carried = settled_end + settled["velocity"] * (time_s - 8.0)
|
|
assert np.linalg.norm(end - carried) < 350.0
|
|
cosine = float(np.dot(settled["velocity"], state["velocity"]) / (
|
|
np.linalg.norm(settled["velocity"]) * np.linalg.norm(state["velocity"])))
|
|
assert cosine > 0.98
|
|
assert index >= 0
|
|
|
|
|
|
def test_long_track_fit_stays_inside_a_physics_step():
|
|
rows = []
|
|
for index, time_s in enumerate(np.arange(0.0, 40.0, 0.05)):
|
|
rows.append({
|
|
"measurement_time_s": float(time_s),
|
|
"position_ned_m": [8000.0 - 140.0 * time_s, 0.0, -1200.0 + 3.0 * time_s],
|
|
"lateral_sigma_m": 40.0, "vertical_sigma_m": 30.0, "accepted": True,
|
|
})
|
|
rows[-1]["position_ned_m"] = [0.0, 6000.0, -4000.0]
|
|
started = time.perf_counter()
|
|
fit = regress_track(rows, rows[-2]["measurement_time_s"])
|
|
assert time.perf_counter() - started < 0.02
|
|
assert fit["velocity"][0] == pytest.approx(-140.0, abs=8.0)
|
|
assert abs(fit["velocity"][1]) < 8.0
|
|
|
|
|
|
def test_infrared_line_is_refit_a_few_times_per_second(monkeypatch):
|
|
projectile = _projectile([2000.0, 0.0, -700.0], [-120.0, 0.0, 4.0])
|
|
sim = SimulationEngine(seed=1, projectiles=[projectile], city_circle=CityCircle(np.zeros(3), 500.))
|
|
history = []
|
|
for time_s in np.arange(0.0, 8.0, 0.2):
|
|
history.append({
|
|
"measurement_time_s": float(time_s),
|
|
"position_ned_m": [5000.0 - 120.0 * time_s, 0.0, -800.0],
|
|
"lateral_sigma_m": 8.0, "vertical_sigma_m": 8.0, "accepted": True,
|
|
})
|
|
sim.ir_fix_history["p"] = history
|
|
calls = {"n": 0}
|
|
real = regress_track
|
|
|
|
def counted(*args, **kwargs):
|
|
calls["n"] += 1
|
|
return real(*args, **kwargs)
|
|
|
|
monkeypatch.setattr("defense_sim.simulation.engine.regress_track", counted)
|
|
sim.time_s = 8.0
|
|
for step in range(100):
|
|
sim.time_s = 8.0 + step * 0.01
|
|
sim._infrared_line("p")
|
|
assert calls["n"] <= 3
|
|
|
|
|
|
def test_infrared_projection_is_the_fitted_line():
|
|
rng = np.random.default_rng(5)
|
|
projectile = _projectile([2000.0, 0.0, -700.0], [-120.0, 0.0, 4.0])
|
|
sim = SimulationEngine(seed=1, projectiles=[projectile], city_circle=CityCircle(np.zeros(3), 500.))
|
|
history = []
|
|
for time_s in np.arange(0.0, 6.0, 0.25):
|
|
position = np.array([5000.0 - 120.0 * time_s, 0.0, -800.0 + 4.0 * time_s])
|
|
position += rng.normal(0.0, (6.0, 6.0, 6.0))
|
|
history.append({
|
|
"measurement_time_s": float(time_s), "position_ned_m": position.tolist(),
|
|
"lateral_sigma_m": 8.0, "vertical_sigma_m": 8.0, "accepted": True,
|
|
})
|
|
sim.ir_fix_history["p"] = history
|
|
sim.time_s = 6.0
|
|
first = np.asarray(sim._ir_display_paths()["p"])
|
|
direction = first[-1] - first[0]
|
|
direction /= np.linalg.norm(direction)
|
|
assert direction[0] < -0.97
|
|
assert abs(direction[1]) < 0.08
|
|
for time_s in np.arange(6.3, 8.0, 0.3):
|
|
position = np.array([5000.0 - 120.0 * time_s, 900.0, -200.0])
|
|
sim.ir_fix_history["p"].append({
|
|
"measurement_time_s": float(time_s), "position_ned_m": position.tolist(),
|
|
"lateral_sigma_m": 8.0, "vertical_sigma_m": 8.0, "accepted": True,
|
|
})
|
|
sim.time_s = float(time_s)
|
|
path = np.asarray(sim._ir_display_paths()["p"])
|
|
later = path[-1] - path[0]
|
|
later /= np.linalg.norm(later)
|
|
assert float(np.dot(direction, later)) > 0.98
|
|
assert abs(path[-1, 1]) < 250.0
|