748 lines
35 KiB
Python
748 lines
35 KiB
Python
import json
|
|
import numpy as np
|
|
import pytest
|
|
|
|
from defense_sim.entities.drone import Drone, DroneConfig
|
|
from defense_sim.entities.projectile import Projectile, ProjectileConfig
|
|
from defense_sim.physics.events import sphere_entry_fraction, circle_entry_fraction
|
|
from defense_sim.physics.reference import integrate_reference
|
|
from defense_sim.sensors.tracking import RadarTracker
|
|
from defense_sim.simulation.engine import SimulationEngine
|
|
from defense_sim.statistics.monte_carlo import run_many
|
|
from defense_sim.sensors.models import Observation, SensorConfig
|
|
from defense_sim.guidance.activation import CityCircle
|
|
|
|
|
|
def projectile(position, velocity, radius=0.1, name="p"):
|
|
return Projectile(name, np.array(position, dtype=float), np.array(velocity, dtype=float),
|
|
ProjectileConfig(area_m2=0, collision_radius_m=radius))
|
|
|
|
|
|
def drone(position, name="d", kind="defense_drone"):
|
|
return Drone(name, kind, np.array(position, dtype=float), config=DroneConfig(collision_radius_m=0.1))
|
|
|
|
|
|
def test_contact_root_and_shallow_circle_chord():
|
|
assert sphere_entry_fraction([-1, 0, 0], [1, 0, 0], 0.2) == pytest.approx(0.4)
|
|
assert circle_entry_fraction([-2, .9, -1], [2, .9, -1], [0, 0, 0], 1) is not None
|
|
|
|
|
|
def test_ground_precedes_underground_collision():
|
|
p = projectile([0, 0, -1], [0, 0, 20], radius=.01)
|
|
sim = SimulationEngine(dt=.1, drone_control_rate_hz=10, seed=1, projectiles=[p], drones=[drone([0, 0, 1])])
|
|
sim.step()
|
|
assert [e["type"] for e in sim.events] == ["ground_impact"]
|
|
assert p.position_ned_m[2] == 0
|
|
assert p.velocity_ned_m_s[2] == pytest.approx(sim.events[0]["velocity_ned_m_s"][2])
|
|
|
|
|
|
@pytest.mark.parametrize("reverse", [False, True])
|
|
def test_first_contact_is_order_independent_and_consumes_drone(reverse):
|
|
p = projectile([-1, 0, -10], [200, 0, 0])
|
|
drones = [drone([.5, 0, -10], "later"), drone([0, 0, -10], "earlier")]
|
|
if reverse:
|
|
drones.reverse()
|
|
sim = SimulationEngine(dt=.01, seed=1, projectiles=[p], drones=drones)
|
|
sim.step()
|
|
event = sim.events[0]
|
|
assert event["type"] == "interception" and event["target_id"] == "earlier"
|
|
assert event["time_s"] == pytest.approx(.004, abs=1e-6)
|
|
assert p.position_ned_m[0] == pytest.approx(-.2, abs=1e-5)
|
|
assert not next(d for d in drones if d.entity_id == "earlier").active
|
|
assert sim.trajectory["p"][-1][0] == pytest.approx(p.position_ned_m[0])
|
|
|
|
|
|
def test_idle_ground_support_under_wind():
|
|
d = drone([1, 2, 0])
|
|
sim = SimulationEngine(seed=1, drones=[d])
|
|
sim.environment.wind_ned_m_s = np.array([20., 3., 0.])
|
|
for _ in range(500):
|
|
sim.step()
|
|
assert np.array_equal(d.position_ned_m, [1, 2, 0])
|
|
assert np.array_equal(d.velocity_ned_m_s, np.zeros(3))
|
|
|
|
|
|
def test_scipy_reference_matches_vacuum_ground_time_and_rk4():
|
|
p = projectile([0, 0, -10], [20, 0, 0])
|
|
state = np.r_[p.position_ned_m, p.velocity_ned_m_s]
|
|
reference = integrate_reference(lambda y: p.derivative(y, np.zeros(3)), state, 2, ground_event=True)
|
|
ground_time = np.sqrt(20/9.80665)
|
|
assert reference.t_events[0][0] == pytest.approx(ground_time, abs=1e-9)
|
|
p.step(.1, np.zeros(3))
|
|
assert np.allclose(np.r_[p.position_ned_m, p.velocity_ned_m_s], reference.sol(.1), atol=1e-9)
|
|
|
|
|
|
def measurement(t, radar="r"):
|
|
position = np.array([1000.+10*t, 0, -10.])
|
|
return {"target_id": "p", "radar_id": radar, "measurement_time_s": t,
|
|
"position_ned_m": position, "range_m": np.linalg.norm(position),
|
|
"azimuth_rad": 0., "elevation_rad": np.arctan2(10., position[0]),
|
|
"range_sigma_m": 1., "azimuth_sigma_rad": .001, "elevation_sigma_rad": .001}
|
|
|
|
|
|
def test_tracking_timestamp_covariance_and_velocity_evidence():
|
|
tracker = RadarTracker(acceleration_diffusion=.01)
|
|
first = tracker.update(measurement(0))
|
|
assert not first.velocity_observable
|
|
for t in (1., 2., 3.):
|
|
result = tracker.update(measurement(t))
|
|
assert result.velocity_observable
|
|
assert result.velocity[0] == pytest.approx(10., abs=.1)
|
|
assert np.linalg.eigvalsh(result.covariance).min() > 0
|
|
assert tracker.update(measurement(3)) is None
|
|
assert tracker.update(measurement(1, "other")) is None
|
|
|
|
|
|
def test_mature_tracks_and_event_covariance_serialize_as_strict_json():
|
|
sim = SimulationEngine(seed=1, projectiles=[projectile([1000, 0, -10], [10, 0, 0])])
|
|
for t in (0., 1., 2., 3.):
|
|
sim.time_s = t
|
|
sim.radar_arrivals.append({**measurement(t), 'arrival_time_s': t})
|
|
sim._deliver_radar_tracks()
|
|
assert sim.radar_tracks['p'].velocity_observable
|
|
json.dumps(sim.snapshot(), allow_nan=False)
|
|
|
|
|
|
def test_monte_carlo_outcomes_are_per_projectile_and_cutoffs_censored():
|
|
def factory(seed):
|
|
return SimulationEngine(seed=seed, projectiles=[
|
|
projectile([-1, 0, -10], [200, 0, 0], name="hit"),
|
|
projectile([100, 0, -100], [0, 0, 0], name="unfinished")],
|
|
drones=[drone([0, 0, -10])])
|
|
result = run_many(factory, runs=2, steps=1)
|
|
groups = result.aggregate["interception_by_projectile"]
|
|
assert groups["hit"]["interception_probability"] == 1
|
|
assert groups["unfinished"]["interception_probability"] is None
|
|
assert groups["unfinished"]["unresolved_count"] == 2
|
|
json.dumps(result.aggregate, allow_nan=False)
|
|
|
|
|
|
def launch_fixture(loss=0):
|
|
from defense_sim.sensors.devices import DroneLinkConfig
|
|
p = projectile([1000, 0, -20], [-50, 0, 0])
|
|
from defense_sim.entities.projectile import ProjectileGuidanceConfig
|
|
p.guidance = ProjectileGuidanceConfig(np.array([0., 0., 0.]), max_thrust_m_s2=20.)
|
|
p.guidance_active = True
|
|
drones = [drone([500, i*5, 0], f'd{i}') for i in range(5)]
|
|
sim = SimulationEngine(seed=3, projectiles=[p], drones=drones,
|
|
city_circle=CityCircle(np.zeros(3), 500),
|
|
sensor_config=SensorConfig(position_sigma_m=0, velocity_sigma_m_s=0),
|
|
drone_link_config=DroneLinkConfig(packet_loss_probability=loss))
|
|
track = Observation('p', 0., p.position_ned_m.copy(), p.velocity_ned_m_s.copy(),
|
|
np.zeros(3), np.zeros(3), np.eye(6), 5, True)
|
|
sim.radars = [] # this fixture isolates explicitly injected tracking evidence
|
|
sim.radar_tracks['p'] = track
|
|
sim.remote_drone_estimates = {d.entity_id: d.position_ned_m.copy() for d in drones}
|
|
sim.remote_drone_estimate_times = {d.entity_id: 0. for d in drones}
|
|
return sim
|
|
|
|
|
|
def seed_stable_inbound(sim, position=(1000., 0., -20.), velocity=(-50., 0., 0.)):
|
|
"""Four accepted samples over 2.1 s, the regression's stable-heading gate."""
|
|
position = np.asarray(position, dtype=float)
|
|
velocity = np.asarray(velocity, dtype=float)
|
|
t0 = sim.time_s
|
|
sim.radar_sample_history['r'] = [{
|
|
'target_id': 'p', 'measurement_time_s': t0 - 2.1 + step,
|
|
'position_ned_m': (position + velocity * (t0 - 2.1 + step)).tolist(),
|
|
'lateral_sigma_m': 40.0, 'vertical_sigma_m': 25.0, 'accepted': True,
|
|
} for step in (0.0, 0.7, 1.4, 2.1)]
|
|
|
|
|
|
def test_decent_regression_assigns_before_filter_velocity_is_observable():
|
|
sim = launch_fixture()
|
|
sim.radar_tracks['p'].velocity_observable = False
|
|
sim.radar_sample_history['r'] = [{
|
|
'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,
|
|
} for time_s in (0.0, 0.7, 1.4, 2.1)]
|
|
sim.radar_sample_history['r'].append({
|
|
'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,
|
|
})
|
|
sim.time_s = 2.2
|
|
sim.remote_drone_estimate_times = {}
|
|
sim.remote_drone_estimates = {}
|
|
sim._activate_defenders(sim.radar_tracks)
|
|
assert 'p' in sim.defense_assignments
|
|
assert len(sim.defense_assignments['p']['defender_ids']) == 1
|
|
assigned = next(e for e in sim.events if e['type'] == 'defenders_assigned')
|
|
assert assigned['forecast_source'] == 'weighted_sample_regression'
|
|
|
|
|
|
def test_climb_limited_frontrun_meets_a_gliding_attacker():
|
|
"""A drone on the 12 km ring, sent when the inbound radar first sees a glide."""
|
|
from defense_sim.entities.drone import Drone, DroneConfig
|
|
from defense_sim.guidance.trajectory import frontrun_station
|
|
cfg = DroneConfig(max_speed_m_s=25., max_climb_rate_m_s=12., max_descent_rate_m_s=8.)
|
|
drone = Drone("d", "defense_drone", np.array([12000., 80., 0.]), config=cfg)
|
|
attacker = np.array([32000., 0., -1500.])
|
|
velocity = np.array([-150., 0., 150. * 1500. / 32000.])
|
|
station, time_s = frontrun_station(
|
|
drone.position_ned_m, attacker, velocity, cfg.max_speed_m_s, lead_s=20.,
|
|
climb_rate_m_s=cfg.max_climb_rate_m_s, descent_rate_m_s=cfg.max_descent_rate_m_s,
|
|
feasible=lambda horizontal, vertical: drone.thrust_supports_velocity(horizontal, vertical, 0.))
|
|
assert -station[2] < cfg.max_climb_rate_m_s * time_s
|
|
drone.flight_reference = {"anchor": station.copy(), "direction": np.array([-1., 0., 0.]),
|
|
"frontrun": True, "elapsed_s": 0., "travel_m": 0., "previous_position": drone.position_ned_m.copy(),
|
|
"initial_altitude_m": 0., "target_altitude_m": float(-station[2]), "speed_m_s": 25.,
|
|
"ascent_distance_m": 1000., "ascent_k_per_m": .002, "command_velocity_ned_m_s": None}
|
|
drone.target_position_ned_m = drone.position_ned_m.copy()
|
|
drone.activated_for = "p"
|
|
rng = np.random.default_rng(0)
|
|
dt = 0.02
|
|
best = 1e9
|
|
for step in range(int((time_s + 5.) / dt)):
|
|
drone.step(dt, np.zeros(3), rng, True, dt)
|
|
attacker = attacker + velocity * dt
|
|
best = min(best, float(np.linalg.norm(drone.position_ned_m - attacker)))
|
|
if attacker[0] < station[0] - 500.:
|
|
break
|
|
assert best < 1.0
|
|
|
|
|
|
def test_frontrun_station_is_further_down_track_and_reachable_early():
|
|
from defense_sim.guidance.trajectory import frontrun_station, intercept_station
|
|
drone = np.array([0., 100., -1500.])
|
|
attacker = np.array([4000., 0., -1500.])
|
|
velocity = np.array([-120., 0., 0.])
|
|
early, early_t = intercept_station(drone, attacker, velocity, 25.)
|
|
front, front_t = frontrun_station(drone, attacker, velocity, 25., lead_s=20.)
|
|
assert front_t > early_t + 5.
|
|
assert front[0] < early[0]
|
|
assert abs(front[1]) < 1.
|
|
flight = np.linalg.norm(front - drone) / 25.
|
|
assert flight + 19. <= front_t + 1e-6
|
|
|
|
|
|
def test_stable_ground_track_picks_the_one_drone_already_ahead():
|
|
sim = launch_fixture()
|
|
places = {
|
|
"d0": np.array([8000., -40., 0.]), # ahead, nearest the y=0 track
|
|
"d1": np.array([9000., 800., 0.]), # ahead but far off the track
|
|
"d2": np.array([20000., 0., 0.]), # on the track, behind the attacker
|
|
"d3": np.array([7000., 200., 0.]),
|
|
}
|
|
for drone in sim.drones:
|
|
if drone.entity_id in places:
|
|
drone.position_ned_m = places[drone.entity_id].copy()
|
|
positions = {drone.entity_id: drone.position_ned_m.copy() for drone in sim.drones if drone.entity_id in places}
|
|
drones = [drone for drone in sim.drones if drone.entity_id in places]
|
|
track = np.array([12000., 0., -1500.])
|
|
velocity = np.array([-120., 0., 0.])
|
|
picked = sim._pick_path_drone(drones, positions, track, velocity)
|
|
assert picked.entity_id == "d0"
|
|
_, references = sim._intercept_plan([picked], positions, track, velocity, None)
|
|
ref = references["d0"]
|
|
assert ref["path_follow"] and ref["direction"][0] < 0
|
|
on_line = picked._path_command(np.array([8000., 0., -1500.]), ref)
|
|
assert on_line[0] < -20.0
|
|
assert abs(on_line[1]) < 1.0
|
|
|
|
|
|
def test_liftoff_runs_along_the_forecast_and_bends_onto_it():
|
|
"""Pitch eases up along the prediction, then the climb holds until the line is close."""
|
|
sim = launch_fixture()
|
|
runner = next(drone for drone in sim.drones if drone.entity_id == "d0")
|
|
runner.config.max_climb_rate_m_s = 12.0
|
|
runner.position_ned_m = np.array([8000., 0., 0.])
|
|
_, references = sim._intercept_plan(
|
|
[runner], {"d0": runner.position_ned_m.copy()},
|
|
np.array([12000., 0., -1500.]), np.array([-120., 0., 0.]), None)
|
|
ref = references["d0"]
|
|
ref["curve_origin"] = runner.position_ned_m.copy()
|
|
corner = float(ref["ascent_distance_m"])
|
|
direction = np.asarray(ref["direction"], dtype=float)
|
|
assert corner == pytest.approx(500.0)
|
|
pitch_up = 160.0
|
|
liftoff = runner._path_command(runner.position_ned_m, ref)
|
|
assert liftoff[0] < -20.0
|
|
assert abs(liftoff[2]) < 0.5
|
|
half = runner.position_ned_m + direction * (pitch_up / 2.0)
|
|
easing = runner._path_command(half, ref)
|
|
assert easing[0] < -15.0
|
|
assert -11.0 < easing[2] < -4.0
|
|
holding = runner.position_ned_m + direction * (pitch_up + 200.0)
|
|
climbing = runner._path_command(holding, ref)
|
|
assert climbing[0] < -12.0
|
|
assert climbing[2] < -10.0
|
|
joined_here = runner.position_ned_m + direction * (corner + 400.0)
|
|
joined_here[2] = -1500.0
|
|
joined = runner._path_command(joined_here, ref)
|
|
assert joined[0] < -20.0
|
|
assert abs(joined[2]) < 1.5
|
|
angles = []
|
|
for fraction in np.linspace(0.0, 1.0, 21):
|
|
point = runner.position_ned_m + direction * (pitch_up * fraction)
|
|
command = runner._path_command(point, ref)
|
|
angles.append(np.arctan2(-command[2], max(float(np.linalg.norm(command[:2])), 1e-6)))
|
|
assert angles[0] == pytest.approx(0.0, abs=0.05)
|
|
assert angles[-1] > np.deg2rad(20.0)
|
|
assert max(abs(np.diff(angles))) < np.deg2rad(12.0)
|
|
assert ref["flare_distance_m"] == pytest.approx(1000.0)
|
|
# 100 m below the line is still a full climb. The flare starts inside 70 m,
|
|
# and that last bend runs at cruise rather than shedding the horizontal speed.
|
|
still_climbing = runner.position_ned_m + direction * 2000.0
|
|
still_climbing[2] = -1400.0
|
|
climbing_late = runner._path_command(still_climbing, ref)
|
|
assert climbing_late[2] < -10.0
|
|
assert ref["keep_climb"] is True
|
|
near = runner.position_ned_m + direction * 2000.0
|
|
near[2] = -1460.0
|
|
joining = runner._path_command(near, ref)
|
|
assert joining[0] < -18.0
|
|
assert joining[2] > -8.0
|
|
assert ref["keep_climb"] is False
|
|
|
|
|
|
def test_an_underground_forecast_makes_the_drone_climb():
|
|
"""A ray that is already below the surface must not be flown into the ground."""
|
|
sim = launch_fixture()
|
|
runner = next(drone for drone in sim.drones if drone.entity_id == "d0")
|
|
runner.config.max_climb_rate_m_s = 12.0
|
|
runner.position_ned_m = np.array([8000., 0., 0.])
|
|
_, references = sim._intercept_plan(
|
|
[runner], {"d0": runner.position_ned_m.copy()},
|
|
np.array([20000., 0., -900.]), np.array([-100., 0., 12.]), None)
|
|
ref = references["d0"]
|
|
ref["curve_origin"] = runner.position_ned_m.copy()
|
|
command = runner._path_command(runner.position_ned_m, ref)
|
|
assert command[2] < -10.0
|
|
point = runner.position_ned_m.astype(float).copy()
|
|
for _ in range(8):
|
|
step = runner._path_command(point, ref)
|
|
point = point + step
|
|
assert point[2] <= 0.0
|
|
assert point[2] < -40.0
|
|
|
|
|
|
def test_a_sideways_offset_is_a_drift_not_a_perpendicular_dash():
|
|
"""A kilometre beside the forecast is closed while still running down the line."""
|
|
sim = launch_fixture()
|
|
runner = next(drone for drone in sim.drones if drone.entity_id == "d0")
|
|
runner.config.max_climb_rate_m_s = 12.0
|
|
runner.position_ned_m = np.array([8000., 1000., 0.])
|
|
_, references = sim._intercept_plan(
|
|
[runner], {"d0": runner.position_ned_m.copy()},
|
|
np.array([20000., 0., -1500.]), np.array([-140., 0., 8.]), None)
|
|
ref = references["d0"]
|
|
ref["curve_origin"] = runner.position_ned_m.copy()
|
|
command = runner._path_command(runner.position_ned_m, ref)
|
|
direction = np.asarray(ref["direction"], dtype=float)[:2]
|
|
direction /= np.linalg.norm(direction)
|
|
along = float(command[:2] @ direction)
|
|
cross = float(np.linalg.norm(command[:2] - direction * along))
|
|
assert along > 8.0
|
|
assert cross <= along + 1e-6
|
|
assert cross < along
|
|
assert np.degrees(np.arctan2(cross, along)) <= 40.0
|
|
|
|
|
|
def test_impact_and_interception_times_follow_the_estimated_track():
|
|
from defense_sim.guidance.trajectory import time_to_impact_s, time_to_interception_s
|
|
# 1 500 m up, descending at 10 m/s: a minute and a half to the ground.
|
|
assert time_to_impact_s([0., 0., -1500.], [-100., 0., 10.]) == pytest.approx(150.0)
|
|
assert time_to_impact_s([0., 0., -1500.], [-100., 0., 0.]) is None
|
|
station = np.array([8000., 0., -1100.])
|
|
attacker = np.array([14000., 0., -1500.])
|
|
velocity = np.array([-120., 0., 8.])
|
|
# The station is 50 s ahead on the estimated ray.
|
|
assert time_to_interception_s(attacker, velocity, station, [10000., 0., 0.], 25.) == pytest.approx(50.0)
|
|
# Station already passed: the drone is 4 km ahead and the attacker closes at 95 m/s.
|
|
assert time_to_interception_s(attacker, velocity, [16000., 0., -1500.], [10000., 0., 0.], 25.) == pytest.approx(4000. / 95.)
|
|
|
|
|
|
def test_forward_station_uses_the_estimated_speed_and_leads_the_attacker():
|
|
"""The aim point is further along the forecast than the drone is now."""
|
|
sim = launch_fixture()
|
|
runner = next(drone for drone in sim.drones if drone.entity_id == "d0")
|
|
runner.config.max_climb_rate_m_s = 12.0
|
|
runner.config.max_descent_rate_m_s = 8.0
|
|
runner.position_ned_m = np.array([12000., 0., 0.])
|
|
track = np.array([24000., 0., -1600.])
|
|
velocity = np.array([-140., 0., 8.])
|
|
_, references = sim._intercept_plan([runner], {"d0": runner.position_ned_m.copy()}, track, velocity, None)
|
|
ref = references["d0"]
|
|
direction = np.asarray(ref["direction"], dtype=float)[:2]
|
|
drone_along = float((runner.position_ned_m[:2] - track[:2]) @ direction)
|
|
assert ref["evaluated_speed_m_s"] == pytest.approx(float(np.linalg.norm(velocity)))
|
|
assert ref["lead_along_m"] > drone_along + 400.0
|
|
lead = np.asarray(ref["lead_point"], dtype=float)
|
|
offset = lead - track
|
|
assert float(offset @ velocity) > 0.0
|
|
assert float(np.linalg.norm(np.cross(offset, velocity))) < 1.0
|
|
assert lead[2] <= 0.0
|
|
# From the ground the climb still runs along the forecast, toward the
|
|
# lower forward station rather than the attacker's present altitude.
|
|
ref["curve_origin"] = runner.position_ned_m.copy()
|
|
command = runner._path_command(runner.position_ned_m, ref)
|
|
assert command[0] < -10.0
|
|
runner.flight_reference = {**ref, "travel_m": 0.0, "elapsed_s": 0.0,
|
|
"previous_position": runner.position_ned_m.copy()}
|
|
runner.target_position_ned_m = runner.position_ned_m.copy()
|
|
preview = np.asarray(runner.intended_trajectory())
|
|
end_along = float((preview[-1, :2] - track[:2]) @ direction)
|
|
assert end_along > drone_along + 800.0
|
|
assert preview[:, 2].max() <= 0.0
|
|
|
|
|
|
def test_blast_radius_destroys_a_near_pass_without_changing_body_size():
|
|
p = projectile([-40., 0., -20.], [80., 0., 0.])
|
|
d = drone([0., 10., -20.], "d")
|
|
missed = SimulationEngine(dt=.01, seed=1, projectiles=[p], drones=[d])
|
|
missed.scenario_config = {"interceptor_blast_radius_m": 0.}
|
|
for _ in range(80):
|
|
missed.step()
|
|
assert p.active
|
|
assert missed.closest_approach_m["p"] > 5.
|
|
p = projectile([-40., 0., -20.], [80., 0., 0.])
|
|
d = drone([0., 10., -20.], "d")
|
|
hit = SimulationEngine(dt=.01, seed=1, projectiles=[p], drones=[d])
|
|
hit.scenario_config = {"interceptor_blast_radius_m": 20.}
|
|
for _ in range(80):
|
|
hit.step()
|
|
event = next(e for e in hit.events if e["type"] == "interception")
|
|
assert not p.active and not d.active
|
|
assert event["neutralization_assumption"] == "blast_radius"
|
|
assert event["blast_radius_m"] == 20.
|
|
# The flyby bottoms out 10 m off the drone. Detonation waits for that
|
|
# minimum instead of firing on the 20 m sphere.
|
|
assert event["miss_distance_m"] == pytest.approx(10.0, abs=0.5)
|
|
assert hit.projectile_outcomes["p"] == "intercepted"
|
|
assert hit.engagements["p"]["result"] == "intercepted"
|
|
assert hit.engagements["p"]["miss_distance_m"] == pytest.approx(event["miss_distance_m"])
|
|
assert not any(item["type"] == "intercept_miss" for item in hit.events)
|
|
|
|
|
|
def test_a_pass_outside_the_blast_is_a_miss_and_the_attacker_continues():
|
|
p = projectile([-40., 0., -20.], [80., 0., 0.])
|
|
d = drone([0., 10., -20.], "d")
|
|
d.activated_for = "p"
|
|
d.target_position_ned_m = d.position_ned_m.copy()
|
|
sim = SimulationEngine(dt=.01, seed=1, projectiles=[p], drones=[d])
|
|
sim.scenario_config = {"interceptor_blast_radius_m": 3.}
|
|
for _ in range(80):
|
|
sim.step()
|
|
assert p.active and d.active
|
|
event = next(item for item in sim.events if item["type"] == "intercept_miss")
|
|
assert 5. < event["miss_distance_m"] < 20.
|
|
assert event["outcome"] == "attacker_continues"
|
|
assert sim.projectile_outcomes.get("p", "unresolved") == "unresolved"
|
|
attacker = next(item for item in sim.snapshot()["entities"] if item["id"] == "p")
|
|
assert attacker["engagement_result"] == "missed"
|
|
assert attacker["engagement_miss_m"] == pytest.approx(event["miss_distance_m"])
|
|
|
|
|
|
def test_inferred_range_uses_the_defense_track_not_truth():
|
|
p = projectile([0., 0., -100.], [-50., 0., 0.])
|
|
d = drone([0., 0., -100.], "d")
|
|
d.activated_for = "p"
|
|
d.estimated_position_ned_m = np.array([0., 40., -100.])
|
|
sim = SimulationEngine(dt=.01, seed=1, projectiles=[p], drones=[d])
|
|
sim.radar_tracks["p"] = Observation(
|
|
"p", 0., np.array([1000., 0., -100.]), np.array([-50., 0., 0.]),
|
|
np.zeros(3), np.zeros(3), np.eye(6), 4, True)
|
|
row = next(item for item in sim.snapshot()["entities"] if item["id"] == "d")
|
|
assert row["inferred_range_m"] == pytest.approx(float(np.hypot(1000., 40.)), abs=0.5)
|
|
|
|
|
|
def test_directed_track_picks_the_near_side_of_the_ring_not_the_far_side():
|
|
sim = launch_fixture()
|
|
sim.scenario_config = {"drone_radius_km": 12.0}
|
|
places = {
|
|
"d0": np.array([12000., 40., 0.]), # where the inbound track enters the ring
|
|
"d1": np.array([-12000., 0., 0.]), # far side, also on the infinite line
|
|
"d2": np.array([0., 12000., 0.]),
|
|
}
|
|
for drone in sim.drones:
|
|
if drone.entity_id in places:
|
|
drone.position_ned_m = places[drone.entity_id].copy()
|
|
drones = [drone for drone in sim.drones if drone.entity_id in places]
|
|
positions = {drone.entity_id: drone.position_ned_m.copy() for drone in drones}
|
|
track = np.array([30000., 0., -400.])
|
|
velocity = np.array([-150., 0., 40.]) # descending; the line is underground by the far side
|
|
picked = sim._pick_path_drone(drones, positions, track, velocity)
|
|
assert picked.entity_id == "d0"
|
|
far = next(drone for drone in drones if drone.entity_id == "d1")
|
|
_, references = sim._intercept_plan([far], positions, track, velocity, None)
|
|
far.flight_reference = {**references["d1"], "travel_m": 0., "elapsed_s": 0.,
|
|
"previous_position": far.position_ned_m.copy(), "initial_altitude_m": 0.}
|
|
far.target_position_ned_m = far.position_ned_m.copy()
|
|
preview = np.array(far.intended_trajectory())
|
|
assert len(preview) > 2
|
|
assert preview[:, 2].max() <= 0.0
|
|
|
|
|
|
def test_intercept_station_is_ahead_of_the_attacker_not_a_chase():
|
|
from defense_sim.guidance.trajectory import intercept_station
|
|
station, time_s = intercept_station([0., 100., -20.], [2000., 0., -20.], [-80., 0., 0.], 20.)
|
|
assert time_s > 1.
|
|
assert station[0] < 2000.
|
|
assert abs(station[1]) < 1.
|
|
drone_distance = np.linalg.norm(station - np.array([0., 100., -20.]))
|
|
assert drone_distance <= 20. * time_s + 1.
|
|
assert abs(2000. - station[0]) == pytest.approx(80. * time_s, abs=1.)
|
|
|
|
|
|
def test_grounded_drones_are_not_offered_to_the_radar():
|
|
sim = launch_fixture()
|
|
seen = []
|
|
|
|
class Radar:
|
|
def scan_dwell(self, targets, time_s):
|
|
seen.extend(target.entity_id for target in targets)
|
|
return []
|
|
|
|
sim.radars = [Radar()]
|
|
sim.drones[0].position_ned_m[2] = 0.
|
|
sim.drones[1].position_ned_m[2] = -30.
|
|
sim._scan_radars()
|
|
assert sim.drones[0].entity_id not in seen
|
|
assert sim.drones[1].entity_id in seen
|
|
assert 'p' in seen
|
|
|
|
|
|
def test_feedback_rejects_a_return_sitting_on_our_drone():
|
|
sim = launch_fixture()
|
|
friendly = sim.drones[0]
|
|
sim.remote_drone_estimates[friendly.entity_id] = friendly.position_ned_m.copy()
|
|
sim.remote_drone_velocities[friendly.entity_id] = np.zeros(3)
|
|
sim.remote_drone_estimate_times[friendly.entity_id] = sim.time_s
|
|
sim.radar_tracks.clear()
|
|
on_drone = {'target_id': 'p', 'radar_id': 'r', 'measurement_time_s': sim.time_s,
|
|
'arrival_time_s': sim.time_s, 'position_ned_m': friendly.position_ned_m.copy(),
|
|
'sensor_position_ned_m': np.zeros(3),
|
|
'range_m': float(np.linalg.norm(friendly.position_ned_m)),
|
|
'azimuth_rad': 0., 'elevation_rad': 0., 'azimuth_sigma_rad': .01,
|
|
'elevation_sigma_rad': .01, 'range_sigma_m': 10., 'radial_velocity_m_s': 0.}
|
|
sim.radar_arrivals.append(on_drone)
|
|
sim._deliver_radar_tracks()
|
|
assert 'p' not in sim.radar_tracks
|
|
assert sim.friendly_returns_suppressed == 1
|
|
attacker = {**on_drone, 'position_ned_m': np.array([1000., 0., -20.]),
|
|
'range_m': 1000., 'radial_velocity_m_s': -50.}
|
|
sim.radar_arrivals.append(attacker)
|
|
sim._deliver_radar_tracks()
|
|
assert 'p' in sim.radar_tracks
|
|
|
|
|
|
def test_three_ir_bearings_fix_the_target_and_release_the_drones():
|
|
sim = launch_fixture()
|
|
target = sim.projectiles[0]
|
|
target.position_ned_m = np.array([0., 0., -20.])
|
|
offsets = (np.array([500., 0., -20.]), np.array([-250., 430., -20.]), np.array([-250., -430., -20.]))
|
|
for index, (drone, offset) in enumerate(zip(sim.drones[:3], offsets)):
|
|
drone.position_ned_m = offset.copy()
|
|
drone.estimated_position_ned_m = offset.copy()
|
|
drone.activated_for = 'p'
|
|
drone.ir_rng = np.random.default_rng(index + 1)
|
|
drone.config.ir_lock_time_s = 0.
|
|
drone.config.ir_processing_latency_s = 0.
|
|
drone.config.ir_bearing_sigma_rad = 1e-4
|
|
drone.config.ir_position_sigma_m = 1.
|
|
toward = target.position_ned_m - offset
|
|
forward = -toward
|
|
yaw = float(np.arctan2(forward[1], forward[0]))
|
|
drone.orientation_quaternion_wxyz = np.array([np.cos(yaw / 2), 0., 0., np.sin(yaw / 2)])
|
|
sim.time_s = 0.05
|
|
for drone in sim.drones[:3]:
|
|
assert drone.estimate_threat(target, sim.time_s, drone.ir_rng, sim.events, None)
|
|
sim._fuse_infrared()
|
|
fix = np.asarray(sim.ir_tracks['p']['position_ned_m'])
|
|
assert np.linalg.norm(fix - target.position_ned_m) < 25.
|
|
assert all(drone.autonomous for drone in sim.drones[:3])
|
|
assert sim.drones[3].autonomous is False
|
|
snapshot = sim.snapshot()
|
|
assert set(snapshot['ir_camera_points']) == {drone.entity_id for drone in sim.drones[:3]}
|
|
assert snapshot['ir_tracks']['p']['cameras'] == 3
|
|
assert snapshot['ir_fixes']['p']
|
|
|
|
|
|
def test_a_settled_track_replans_the_same_drones():
|
|
sim = launch_fixture()
|
|
sim.radar_tracks['p'].velocity_observable = True
|
|
sim.time_s = 1.
|
|
seed_stable_inbound(sim)
|
|
sim.remote_drone_estimate_times = {d.entity_id: 1. for d in sim.drones}
|
|
sim._activate_defenders(sim.radar_tracks)
|
|
assigned = list(sim.defense_assignments['p']['defender_ids'])
|
|
sim.time_s = max(c['delivery_time_s'] for c in sim.pending_drone_commands)+.01
|
|
sim._deliver_drone_commands()
|
|
drone = next(d for d in sim.drones if d.entity_id == assigned[0])
|
|
old_direction = drone.flight_reference['direction'].copy()
|
|
sim.radar_tracks['p'].velocity_ned_m_s = np.array([-40., 15., 0.])
|
|
sim.radar_tracks['p'].timestamp_s = sim.time_s
|
|
sim.time_s += 2.
|
|
seed_stable_inbound(sim, velocity=(-40., 15., 0.))
|
|
sim._forecast_cache.pop('p', None)
|
|
sim._radar_fit_cache.pop('p', None)
|
|
sim._service_assignments()
|
|
assert any(c['drone_id'] in assigned for c in sim.pending_drone_commands)
|
|
sim.time_s = max(c['delivery_time_s'] for c in sim.pending_drone_commands)+.01
|
|
sim._deliver_drone_commands()
|
|
assert drone.flight_reference['direction'][1] > 0.05
|
|
assert float(np.dot(old_direction, drone.flight_reference['direction'])) < 0.999
|
|
assert set(sim.defense_assignments['p']['defender_ids']) == set(assigned)
|
|
|
|
|
|
def test_launch_requires_evidence_and_delivered_commands():
|
|
sim = launch_fixture()
|
|
sim.radar_tracks['p'].velocity_observable = False
|
|
sim._activate_defenders(sim.radar_tracks)
|
|
assert all(d.available for d in sim.drones)
|
|
sim.radar_tracks['p'].velocity_observable = True
|
|
sim.time_s = 1.
|
|
seed_stable_inbound(sim)
|
|
sim.remote_drone_estimate_times = {d.entity_id: 1. for d in sim.drones}
|
|
sim._activate_defenders(sim.radar_tracks)
|
|
assert sum(not d.available for d in sim.drones) == 1
|
|
assert all(d.activated_for is None and d.target_position_ned_m is None for d in sim.drones)
|
|
sim.time_s = max(c['delivery_time_s'] for c in sim.pending_drone_commands)+.01
|
|
sim._deliver_drone_commands()
|
|
assert sum(d.activated_for == 'p' for d in sim.drones) == 1
|
|
runner = next(d for d in sim.drones if d.activated_for == 'p')
|
|
start = runner.position_ned_m.copy()
|
|
for _ in range(500):
|
|
sim.step()
|
|
assert runner.position_ned_m[0] < start[0] - 20.
|
|
assert all(d.position_ned_m[2] <= 0 for d in sim.drones)
|
|
|
|
|
|
def test_total_packet_loss_retries_and_releases_reservations():
|
|
sim = launch_fixture(loss=1.)
|
|
seed_stable_inbound(sim)
|
|
sim._activate_defenders(sim.radar_tracks)
|
|
for t in (1., 2., 3.):
|
|
sim.time_s = t
|
|
sim._service_assignments()
|
|
assert not sim.defense_assignments
|
|
assert all(d.available and d.activated_for is None for d in sim.drones)
|
|
assert len([e for e in sim.events if e['type'] == 'drone_command_dropped']) == 3
|
|
|
|
|
|
def test_ir_bearing_only_processing_delay_and_loss():
|
|
d = drone([0, 0, -10])
|
|
d.activated_for = 'p'
|
|
d.ir_rng = np.random.default_rng(1)
|
|
d.config.ir_lock_time_s = 0.
|
|
d.config.ir_processing_latency_s = .05
|
|
p = projectile([-100, 0, -10], [0, 0, 0])
|
|
prior = Observation('p', 0., p.position_ned_m.copy(), np.zeros(3), np.zeros(3),
|
|
np.zeros(3), np.eye(6)*100, 5, True)
|
|
events = []
|
|
assert not d.estimate_threat(p, 0., d.ir_rng, events, prior)
|
|
assert d.estimate_threat(p, .05, d.ir_rng, events, prior)
|
|
assert d.ir_observation['measurement_type'] == 'ir_camera_position'
|
|
assert d.ir_observation['drone_id'] == d.entity_id
|
|
assert np.linalg.norm(np.asarray(d.ir_observation['position_ned_m']) - p.position_ned_m) < 40
|
|
p.position_ned_m = np.array([100., 0., -10.]) # outside rear-facing FOV
|
|
assert not d.estimate_threat(p, .1, d.ir_rng, events, prior)
|
|
assert not d.ir_acquired and d.ir_threat_estimate is None
|
|
|
|
|
|
@pytest.mark.parametrize('dt', [.1, .05, .025])
|
|
def test_ground_event_converges_to_scipy_reference(dt):
|
|
p = projectile([0, 0, -10], [0, 0, 0])
|
|
sim = SimulationEngine(dt=dt, drone_control_rate_hz=min(50, 1/dt), seed=1, projectiles=[p])
|
|
while p.active:
|
|
sim.step()
|
|
analytic = np.sqrt(20/9.80665)
|
|
assert abs(sim.events[-1]['time_s']-analytic) < dt*dt
|
|
|
|
|
|
def test_atmosphere_high_altitude_and_nonfinite_rejected():
|
|
from defense_sim.physics.atmosphere import isa
|
|
assert 1e-5 < isa(80000).density_kg_m3 < 3e-5
|
|
assert isa(60000).density_kg_m3 < isa(20000).density_kg_m3/100
|
|
with pytest.raises(ValueError):
|
|
DroneConfig(max_speed_m_s=float('nan'))
|
|
with pytest.raises(ValueError):
|
|
isa(90000)
|
|
|
|
|
|
def test_radar_dwell_evaluates_all_targets_and_preserves_clock_phase():
|
|
from defense_sim.sensors.devices import Aeris10Radar, AerisRadarConfig
|
|
config = AerisRadarConfig(range_sigma_m=0, azimuth_sigma_deg=0, elevation_sigma_deg=0, doppler_sigma_m_s=0)
|
|
visible = projectile([1000, 0, -10], [-10, 0, 0], name='visible')
|
|
outside = projectile([30000, 0, -10], [-10, 0, 0], name='outside')
|
|
for targets in ([outside, visible], [visible, outside]):
|
|
radar = Aeris10Radar('r', np.zeros(3), 0, config, np.random.default_rng(3))
|
|
assert [m['target_id'] for m in radar.scan_dwell(targets, 0)] == ['visible']
|
|
for t in np.arange(.01, 13.01, .01):
|
|
radar.scan_dwell([], float(t))
|
|
assert radar.next_sweep_time_s == pytest.approx(51*config.azimuth_step_period_s)
|
|
|
|
|
|
def test_telemetry_out_of_order_cannot_overwrite_newer_sample():
|
|
sim = SimulationEngine(seed=1)
|
|
sim.time_s = 2.
|
|
base = {'drone_id': 'd', 'delivery_time_s': 1., 'wireless_latency_s': .1}
|
|
sim.pending_drone_telemetry = [
|
|
{**base, 'measurement_time_s': 1., 'position_ned_m': np.ones(3)},
|
|
{**base, 'measurement_time_s': .5, 'position_ned_m': np.zeros(3)}]
|
|
sim._deliver_drone_telemetry()
|
|
assert np.array_equal(sim.remote_drone_estimates['d'], np.ones(3))
|
|
assert sim.remote_drone_estimate_times['d'] == 1.
|
|
|
|
|
|
def test_guidance_does_not_overwrite_velocity_or_create_zero_thrust():
|
|
from defense_sim.entities.projectile import ProjectileGuidanceConfig
|
|
p = projectile([1000, 0, -100], [-50, 0, 0])
|
|
p.guidance = ProjectileGuidanceConfig(np.zeros(3), max_thrust_m_s2=5.)
|
|
p.guidance_active = True
|
|
initial = p.velocity_ned_m_s.copy()
|
|
p.update_guidance(0., np.zeros(3))
|
|
assert np.array_equal(p.velocity_ned_m_s, initial)
|
|
assert np.linalg.norm(p._thrust_cmd) <= 5.+1e-12
|
|
p.guidance.max_thrust_m_s2 = 0.
|
|
p.update_guidance(1., np.zeros(3))
|
|
assert np.array_equal(p._thrust_cmd, np.zeros(3))
|
|
|
|
|
|
def test_ground_wins_tie_and_other_drone_contact_is_not_defense_success():
|
|
p = projectile([0, 0, 0], [0, 0, 0])
|
|
sim = SimulationEngine(seed=1, projectiles=[p], drones=[drone([0, 0, 0])])
|
|
sim.step()
|
|
assert sim.events[0]['type'] == 'ground_impact'
|
|
q = projectile([-1, 0, -10], [200, 0, 0])
|
|
sim = SimulationEngine(seed=1, projectiles=[q], drones=[drone([0, 0, -10], kind='attack_drone')])
|
|
sim.step()
|
|
assert sim.events[0]['type'] == 'collision'
|
|
assert sim.projectile_outcomes['p'] == 'collision'
|
|
|
|
|
|
@pytest.mark.parametrize('reverse', [False, True])
|
|
def test_consumed_drone_cannot_contact_later_threat(reverse):
|
|
first = projectile([-.8, 0, -10], [200, 0, 0], name='first')
|
|
later = projectile([-1., 0, -10], [200, 0, 0], name='later')
|
|
threats = [later, first]
|
|
if reverse:
|
|
threats.reverse()
|
|
sim = SimulationEngine(seed=1, projectiles=threats, drones=[drone([0, 0, -10])])
|
|
sim.step()
|
|
assert not first.active and later.active
|
|
assert [e['projectile_id'] for e in sim.events if e['type'] == 'interception'] == ['first']
|
|
|
|
|
|
def test_high_altitude_track_still_commands_projected_defense_paths():
|
|
"""A jet-munition altitude fails the full-arrival screen and must still be flown."""
|
|
sim = launch_fixture()
|
|
sim.projectiles[0].position_ned_m = np.array([1000., 0., -2500.])
|
|
sim.projectiles[0].velocity_ned_m_s = np.array([-50., 0., 0.])
|
|
track = sim.radar_tracks['p']
|
|
track.position_ned_m = sim.projectiles[0].position_ned_m.copy()
|
|
track.velocity_ned_m_s = sim.projectiles[0].velocity_ned_m_s.copy()
|
|
track.velocity_observable = True
|
|
sim.time_s = 1.
|
|
seed_stable_inbound(sim, position=(1000., 0., -2500.), velocity=(-50., 0., 0.))
|
|
sim.remote_drone_estimate_times = {d.entity_id: 1. for d in sim.drones}
|
|
sim._activate_defenders(sim.radar_tracks)
|
|
assert any(e['type'] == 'defense_assignment_infeasible' for e in sim.events)
|
|
assert 'p' in sim.defense_assignments
|
|
assert len(sim.pending_drone_commands) == 1
|
|
refs = sim.defense_assignments['p']['references']
|
|
assert all(ref['target_altitude_m'] == pytest.approx(2500.) for ref in refs.values())
|
|
assert all(ref['direction'][0] < 0 for ref in refs.values())
|