119 lines
5.6 KiB
Python
119 lines
5.6 KiB
Python
import numpy as np
|
|
import json
|
|
|
|
from defense_sim.entities.drone import Drone, DroneConfig
|
|
from defense_sim.entities.projectile import Projectile, ProjectileConfig
|
|
from defense_sim.physics.atmosphere import isa
|
|
from defense_sim.physics.integrator import rk4_step
|
|
from defense_sim.physics.turbulence import DrydenGust
|
|
from defense_sim.simulation.engine import SimulationEngine
|
|
from defense_sim.guidance.activation import CityCircle, first_circle_crossing, select_defenders
|
|
from defense_sim.sensors.models import Sensor, SensorConfig
|
|
from defense_sim.statistics.metrics import summarize_runs
|
|
|
|
|
|
def test_isa_sea_level_and_density_decreases():
|
|
sea = isa(0)
|
|
assert sea.temperature_k == 288.15
|
|
assert abs(sea.pressure_pa - 101325) < 1
|
|
assert abs(sea.density_kg_m3 - 1.225) < 0.002
|
|
assert isa(5000).density_kg_m3 < sea.density_kg_m3
|
|
|
|
|
|
def test_rk4_constant_acceleration_matches_analytic():
|
|
state = np.array([0.0, 0.0])
|
|
result = rk4_step(lambda y: np.array([y[1], 3.0]), state, 0.2)
|
|
assert np.allclose(result, [0.06, 0.6], atol=1e-12)
|
|
|
|
|
|
def test_projectile_vacuum_vertical_motion_and_drag_reduces_speed():
|
|
p = Projectile("p", np.zeros(3), np.array([10.0, 0.0, 0.0]), ProjectileConfig(area_m2=0))
|
|
p.step(0.1, np.zeros(3))
|
|
assert abs(p.position_ned_m[0] - 1.0) < 1e-8
|
|
assert abs(p.velocity_ned_m_s[2] - 0.980665) < 1e-8
|
|
q = Projectile("q", np.zeros(3), np.array([100.0, 0.0, 0.0]), ProjectileConfig())
|
|
q.step(0.1, np.zeros(3))
|
|
assert q.velocity_ned_m_s[0] < 100
|
|
|
|
|
|
def test_drone_actuator_and_turn_rate_are_bounded():
|
|
cfg = DroneConfig(max_acceleration_m_s2=2, max_turn_rate_rad_s=0.2, acceleration_time_constant_s=0.01)
|
|
d = Drone("d", "defense_drone", np.zeros(3), config=cfg, target_position_ned_m=np.array([0., 100., 0.]))
|
|
d.estimated_position_ned_m = d.position_ned_m.copy()
|
|
d.estimated_velocity_ned_m_s = d.velocity_ned_m_s.copy()
|
|
d.step(0.1, np.zeros(3), np.random.default_rng(1))
|
|
assert np.linalg.norm(d.actuator_acceleration_ned_m_s2) <= 2 + 1e-9
|
|
assert abs(d.angular_velocity_body_rad_s[2]) <= 0.2 + 1e-9
|
|
|
|
|
|
def test_dryden_is_seed_reproducible_and_finite():
|
|
a = DrydenGust(np.random.default_rng(19), (1, 1, 1))
|
|
b = DrydenGust(np.random.default_rng(19), (1, 1, 1))
|
|
assert np.allclose(a.update(0.01, 20), b.update(0.01, 20))
|
|
assert np.isfinite(a.value).all()
|
|
|
|
|
|
def test_seeded_engine_snapshots_reproduce_and_separate_estimate_error():
|
|
def create():
|
|
return SimulationEngine(seed=3, drones=[Drone("d", "attack_drone", np.array([0., 0., -10.]))])
|
|
x, y = create(), create()
|
|
sx, sy = x.step_and_snapshot(), y.step_and_snapshot()
|
|
assert sx["entities"] == sy["entities"]
|
|
d = sx["entities"][0]
|
|
assert d["position_error_ned_m"] is not None
|
|
assert d["orientation_quaternion_wxyz"][0] >= 0
|
|
|
|
|
|
def test_swept_interception_detects_crossing_between_steps():
|
|
p = Projectile("p", np.array([-1., 0., -10.]), np.array([200., 0., 0.]),
|
|
ProjectileConfig(area_m2=0, collision_radius_m=0.1))
|
|
d = Drone("d", "defense_drone", np.array([0., 0., -10.]),
|
|
velocity_ned_m_s=np.array([0., 0., 0.]), config=DroneConfig(collision_radius_m=0.1))
|
|
sim = SimulationEngine(dt=0.01, seed=1, projectiles=[p], drones=[d])
|
|
sim.step()
|
|
assert sim.events and sim.events[0]["type"] == "interception"
|
|
assert sim.events[0]["miss_distance_m"] <= 0.2 + 1e-12
|
|
|
|
|
|
def test_error_sign_and_monte_carlo_aggregation_shape():
|
|
d = Drone("d", "defense_drone", np.array([2., 0., 0.]), estimated_position_ned_m=np.array([1., 0., 0.]))
|
|
assert np.allclose(d.position_ned_m - d.estimated_position_ned_m, [1, 0, 0])
|
|
stats = summarize_runs([{"projectile_id": "p1", "intercepted": True, "miss_distance_m": 0.1,
|
|
"interception_time_s": 2.0},
|
|
{"projectile_id": "p1", "intercepted": False, "miss_distance_m": 3.0}])
|
|
assert stats["overall"]["interception_probability"] == 0.5
|
|
assert stats["interception_by_projectile"]["p1"]["run_count"] == 2
|
|
|
|
|
|
def test_projected_first_city_circle_crossing_and_three_nearest_defenders():
|
|
p = Projectile("threat", np.array([1000., 0., -1000.]), np.array([-50., 0., 0.]),
|
|
ProjectileConfig(area_m2=0))
|
|
city = CityCircle(np.zeros(3), 500.0)
|
|
crossing = first_circle_crossing(p, city, np.zeros(3), dt=0.1)
|
|
assert crossing is not None
|
|
assert abs(np.linalg.norm(crossing[:2]) - 500.0) < 1e-6
|
|
positions = [[500., 0., -1000.], [500., 100., -1000.],
|
|
[500., -100., -1000.], [0., 0., -1000.]]
|
|
drones = [Drone(f"d{i}", "defense_drone", np.array(pos)) for i, pos in enumerate(positions)]
|
|
# The selection rule ranks available defense drones by 3D distance to the
|
|
# predicted inward crossing point.
|
|
chosen = select_defenders(drones, crossing, count=3)
|
|
assert [d.entity_id for d in chosen] == ["d0", "d1", "d2"]
|
|
|
|
|
|
def test_sensor_latency_uses_historical_ground_truth_and_packet_serializes():
|
|
d = Drone("d", "attack_drone", np.array([0., 0., -10.]))
|
|
sensor = Sensor(SensorConfig(position_sigma_m=0, velocity_sigma_m_s=0, latency_s=0.2),
|
|
np.random.default_rng(2))
|
|
assert sensor.observe(d, 0.0) is None
|
|
d.position_ned_m = np.array([10., 0., -10.])
|
|
assert sensor.observe(d, 0.1) is None
|
|
delayed = sensor.observe(d, 0.2)
|
|
assert np.allclose(delayed.position_ned_m, [0, 0, -10])
|
|
d.position_ned_m = np.array([20., 0., -10.])
|
|
current = sensor.observe(d, 0.3)
|
|
assert np.allclose(current.position_ned_m, [10, 0, -10])
|
|
assert current.timestamp_s == 0.1
|
|
packet = SimulationEngine(seed=4, drones=[d]).step_and_snapshot()
|
|
json.dumps(packet, allow_nan=False)
|