def test_rotation_offsets_3d(state, expected_platform_orientation,
expected_sensor_orientations, move, radars_3d,
rotation_offsets_3d):
# Define time related variables
timestamp = datetime.datetime.now()
# Define transition model and position for platform
model_1d = ConstantVelocity(0.0) # zero noise so pure movement
trans_model = CombinedLinearGaussianTransitionModel(
[model_1d] * (radars_3d[0].ndim_state // 2))
platform_state = State(state, timestamp)
# This defines the position_mapping to the platforms state vector (i.e. x and y)
mounting_mapping = np.array([0, 2, 4])
# create a platform with the simple radar mounted
for sensor, offset in zip(radars_3d, rotation_offsets_3d):
sensor.rotation_offset = offset
platform = MovingPlatform(states=platform_state,
transition_model=trans_model,
sensors=radars_3d,
position_mapping=mounting_mapping)
if move:
# Move the platform
platform.move(timestamp + datetime.timedelta(seconds=2))
assert np.allclose(platform.orientation, expected_platform_orientation)
assert np.allclose(all_sensor_orientations(platform),
expected_sensor_orientations)
def test_setting_orientation():
timestamp = datetime.datetime.now()
platform_state = State(np.array([[2], [2], [0]]), timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=None,
position_mapping=[0, 1, 2])
with pytest.raises(AttributeError):
platform.orientation = [0, 0, 0]
platform_state = State(np.array([[2], [2], [0]]), timestamp)
platform_orientation = StateVector([0, 0, 0])
platform = FixedPlatform(states=platform_state,
position_mapping=[0, 1, 2],
orientation=platform_orientation)
assert np.array_equal(platform.orientation, StateVector([0, 0, 0]))
platform.orientation = StateVector([0, 1, 0])
assert np.array_equal(platform.orientation, StateVector([0, 1, 0]))
platform_state = State(np.array([[2], [1], [2], [-1], [2], [0]]),
timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=None,
position_mapping=[0, 1, 2])
with pytest.raises(AttributeError):
platform.orientation = [0, 0]
platform_state = State(np.array([[2], [1], [2], [-1], [2], [0]]),
timestamp)
platform_orientation = StateVector([0, 0, 0])
platform = FixedPlatform(states=platform_state,
position_mapping=[0, 2, 4],
orientation=platform_orientation)
assert np.array_equal(platform.orientation, StateVector([0, 0, 0]))
platform.orientation = StateVector([0, 1, 0])
assert np.array_equal(platform.orientation, StateVector([0, 1, 0]))
def test_moving_with_no_initial_timestamp():
timestamp = datetime.datetime.now()
platform_state = State(StateVector([2, 1, 1, 1, 2, 0]), timestamp=None)
platform = MovingPlatform(states=platform_state,
position_mapping=[0, 2, 4],
transition_model=None)
assert platform.timestamp is None
platform.move(timestamp=timestamp)
assert platform.timestamp == timestamp
def test_platform_getitem():
timestamp = datetime.datetime.now()
state_before = State(np.array([[2], [1], [2], [1], [0], [1]]), timestamp)
cv_model = CombinedLinearGaussianTransitionModel(
(ConstantVelocity(0), ConstantVelocity(0), ConstantVelocity(0)))
platform = MovingPlatform(states=state_before,
transition_model=cv_model,
position_mapping=[0, 2, 4],
velocity_mapping=[1, 3, 5])
platform.move(timestamp + datetime.timedelta(seconds=1))
state_after = platform.state
assert platform[0] is state_before
assert platform[1] is state_after
def test_2d_platform(state, expected, move, radars_2d, mounting_offsets_2d,
add_sensor):
# Define time related variables
timestamp = datetime.datetime.now()
# Define transition model and position for platform
model_1d = ConstantVelocity(0.0) # zero noise so pure movement
trans_model = CombinedLinearGaussianTransitionModel(
[model_1d] * (radars_2d[0].ndim_state // 2))
platform_state = State(state, timestamp)
# This defines the position_mapping to the platforms state vector (i.e. x and y)
mounting_mapping = np.array([0, 2])
# create a platform with the simple radar mounted
for sensor, offset in zip(radars_2d, mounting_offsets_2d):
sensor.mounting_offset = offset
if add_sensor:
platform = MovingPlatform(states=platform_state,
transition_model=trans_model,
sensors=[],
position_mapping=mounting_mapping)
for sensor in radars_2d:
platform.add_sensor(sensor)
else:
platform = MovingPlatform(states=platform_state,
transition_model=trans_model,
sensors=radars_2d,
position_mapping=mounting_mapping)
if move:
# Move the platform
platform.move(timestamp + datetime.timedelta(seconds=2))
sensor_positions_test(expected, platform)
def test_range_and_angles_to_other(first_state, second_state,
expected_measurement):
# Note that, due to platform orientation, range_and_angles_to_other is not symmetric, so we
# cannot test simple inversion here.
timestamp = datetime.datetime.now()
platform1 = MovingPlatform(states=State(first_state, timestamp=timestamp),
position_mapping=(0, 2, 4),
transition_model=None)
platform2 = MovingPlatform(states=State(second_state, timestamp=timestamp),
position_mapping=(0, 2, 4),
transition_model=None)
range_, azimuth, elevation = platform1.range_and_angles_to_other(platform2)
assert np.allclose((elevation, azimuth, range_), expected_measurement[0:3])
def test_setting_movement_controller():
timestamp = datetime.datetime.now()
fixed_state = State(np.array([[2], [2], [0]]), timestamp)
fixed = FixedMovable(states=fixed_state, position_mapping=(0, 1, 2))
platform = MovingPlatform(movement_controller=fixed)
assert np.array_equal(platform.position, StateVector([2, 2, 0]))
assert np.array_equal(platform.velocity, StateVector([0, 0, 0]))
moving_state = State(np.array([[2], [1], [2], [-1], [2], [0]]), timestamp)
moving = MovingMovable(states=moving_state,
position_mapping=(0, 2, 4),
transition_model=None)
platform = MovingPlatform(movement_controller=moving)
assert np.array_equal(platform.position, StateVector([2, 2, 2]))
assert np.array_equal(platform.velocity, StateVector([1, -1, 0]))
def test_platform_orientation_2d(state, orientation):
model_1d = ConstantVelocity(0.0)
model_2d = CombinedLinearGaussianTransitionModel([model_1d, model_1d])
timestamp = datetime.datetime.now()
timediff = 2 # 2sec
new_timestamp = timestamp + datetime.timedelta(seconds=timediff)
platform_state = State(state, timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=model_2d,
position_mapping=[0, 2])
assert np.allclose(platform.orientation, orientation)
# moving with a constant velocity model should not change the orientation
platform.move(new_timestamp)
assert np.allclose(platform.orientation, orientation)
def test_orientation_dimensionality_error():
platform_state = State(StateVector([2, 1, 1, 1, 2, 0, 1, 0]),
timestamp=datetime.datetime.now())
platform = MovingPlatform(states=platform_state,
position_mapping=[0, 1, 2, 3],
transition_model=None)
with pytest.raises(NotImplementedError):
_ = platform.orientation
platform = MovingPlatform(states=platform_state,
position_mapping=[0],
transition_model=None)
with pytest.raises(NotImplementedError):
_ = platform.orientation
def test_orientation_error():
# moving platform without velocity defined
timestamp = datetime.datetime.now()
platform_state = State(np.array([[2], [2], [0]]), timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=None,
position_mapping=[0, 1, 2])
with pytest.raises(AttributeError):
_ = platform.orientation
platform_state = State(np.array([[2], [0], [2], [0], [2], [0]]), timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=None,
position_mapping=[0, 2, 4])
with pytest.raises(AttributeError):
_ = platform.orientation
platform_state = State(np.array([[2], [0], [2], [0]]), timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=None,
position_mapping=[0, 2])
with pytest.raises(AttributeError):
_ = platform.orientation
def test_mapping_types(mapping_type):
timestamp = datetime.datetime.now()
platform_state = State(np.array([[2], [2], [0]]), timestamp)
platform = FixedPlatform(states=platform_state,
position_mapping=mapping_type([0, 1, 2]))
assert np.array_equal(platform.position, StateVector([2, 2, 0]))
platform.position = StateVector([0, 0, 1])
assert np.array_equal(platform.position, StateVector([0, 0, 1]))
platform_state = State(np.array([[2], [1], [2], [-1], [2], [0]]),
timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=None,
position_mapping=mapping_type([0, 2, 4]))
assert np.array_equal(platform.position, StateVector([2, 2, 2]))
assert np.array_equal(platform.velocity, StateVector([1, -1, 0]))
def test_setting_position():
timestamp = datetime.datetime.now()
platform_state = State(np.array([[2], [2], [0]]), timestamp)
model_1d = ConstantVelocity(0.0)
model_3d = CombinedLinearGaussianTransitionModel(
[model_1d, model_1d, model_1d])
platform = MovingPlatform(states=platform_state,
transition_model=model_3d,
position_mapping=[0, 1, 2])
with pytest.raises(AttributeError):
platform.position = [0, 0, 0]
with pytest.raises(AttributeError):
platform.velocity = [0, 0, 0]
platform_state = State(np.array([[2], [2], [0]]), timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=None,
position_mapping=[0, 1, 2])
assert np.array_equal(platform.position, StateVector([2, 2, 0]))
platform.position = StateVector([0, 0, 0])
assert np.array_equal(platform.position, StateVector([0, 0, 0]))
with pytest.raises(AttributeError):
platform.velocity = [0, 0, 0]
platform_state = State(np.array([[2], [2], [0]]), timestamp)
platform = FixedPlatform(states=platform_state, position_mapping=[0, 1, 2])
assert np.array_equal(platform.position, StateVector([2, 2, 0]))
platform.position = StateVector([0, 0, 0])
assert np.array_equal(platform.position, StateVector([0, 0, 0]))
assert np.array_equal(platform.state_vector, StateVector([0, 0, 0]))
with pytest.raises(AttributeError):
platform.velocity = [0, 0, 0]
platform_state = State(np.array([[2], [1], [2], [-1], [2], [0]]),
timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=model_3d,
position_mapping=[0, 2, 4])
with pytest.raises(AttributeError):
platform.position = [0, 0, 0]
platform_state = State(np.array([[2], [1], [2], [-1], [2], [0]]),
timestamp)
platform = FixedPlatform(states=platform_state, position_mapping=[0, 2, 4])
assert np.array_equal(platform.position, StateVector([2, 2, 2]))
platform.position = StateVector([0, 0, 1])
assert np.array_equal(platform.position, StateVector([0, 0, 1]))
assert np.array_equal(platform.state_vector,
StateVector([0, 1, 0, -1, 1, 0]))
with pytest.raises(AttributeError):
platform.velocity = [0, 0, 0]
platform_state = State(np.array([[2], [1], [2], [-1], [2], [0]]),
timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=None,
position_mapping=[0, 2, 4])
assert np.array_equal(platform.position, StateVector([2, 2, 2]))
platform.position = StateVector([0, 0, 1])
assert np.array_equal(platform.position, StateVector([0, 0, 1]))
assert np.array_equal(platform.state_vector,
StateVector([0, 1, 0, -1, 1, 0]))
with pytest.raises(AttributeError):
platform.velocity = [0, 0, 0]
def test_base():
# Define time related variables
timestamp = datetime.datetime.now()
timediff = 2 # 2sec
new_timestamp = timestamp + datetime.timedelta(seconds=timediff)
# Define a static 2d platform and check it does not move
platform_state2d = State(np.array([[2], [2]]), timestamp)
platform = FixedPlatform(states=platform_state2d,
position_mapping=np.array([0, 1]))
platform.move(new_timestamp)
new_statevector = np.array([[2], [2]])
# Ensure 2d platform has not moved
assert (np.array_equal(platform.state.state_vector, new_statevector))
# Test to ensure platform time has updated
assert (platform.state.timestamp == new_timestamp)
assert np.array_equal(platform.velocity, StateVector([0, 0]))
assert platform.ndim == 2
assert not platform.is_moving
# Define a static 3d platform and check it does not move
platform_state3d = State(np.array([[2], [2], [2]]), timestamp)
platform = FixedPlatform(states=platform_state3d,
position_mapping=[0, 1, 2])
platform.move(new_timestamp)
new_statevector = np.array([[2], [2], [2]])
# Ensure 2d platform has not moved
assert np.array_equal(platform.state.state_vector, new_statevector)
assert np.array_equal(platform.velocity, StateVector([0, 0, 0]))
assert platform.ndim == 3
assert not platform.is_moving
# Define zero noise 2d constant velocity transition model
model_1d = ConstantVelocity(0.0)
model_2d = CombinedLinearGaussianTransitionModel([model_1d, model_1d])
# Define a 2d platform with constant velocity motion and test motion
platform_state2d = State(np.array([[2], [1], [2], [1]]), timestamp)
platform = MovingPlatform(states=platform_state2d,
transition_model=model_2d,
position_mapping=[0, 2])
platform.move(new_timestamp)
# Define expected platform location after movement
new_statevector = np.array([[4], [1], [4], [1]])
assert (np.array_equal(platform.state.state_vector, new_statevector))
assert np.array_equal(platform.velocity, StateVector([1, 1]))
assert platform.ndim == 2
assert platform.is_moving
# Define zero noise 3d constant velocity transition model
model_3d = CombinedLinearGaussianTransitionModel(
[model_1d, model_1d, model_1d])
# Define a 3d platform with constant velocity motion and test motion
platform_state = State(np.array([[2], [1], [2], [1], [0], [1]]), timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=model_3d,
position_mapping=[0, 2, 4])
platform.move(new_timestamp)
# Define expected platform location in 3d after movement
new_statevector = np.array([[4], [1], [4], [1], [2], [1]])
assert (np.array_equal(platform.state.state_vector, new_statevector))
assert np.array_equal(platform.velocity, StateVector([1, 1, 1]))
assert platform.ndim == 3
assert platform.is_moving
def test_velocity_properties(velocity_mapping):
model_1d = ConstantVelocity(0.0)
model_3d = CombinedLinearGaussianTransitionModel(
[model_1d, model_1d, model_1d])
timestamp = datetime.datetime.now()
timediff = 2 # 2sec
new_timestamp = timestamp + datetime.timedelta(seconds=timediff)
platform_state = State(np.array([[2], [0], [2], [0], [0], [0]]), timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=model_3d,
position_mapping=[0, 2, 4])
old_position = platform.position
assert not platform.is_moving
assert np.array_equal(platform.velocity, StateVector([0, 0, 0]))
# check it doesn't move with timestamp = None
platform.move(timestamp=None)
assert np.array_equal(platform.position, old_position)
# check it doesn't move (as it has zero velocity)
platform.move(timestamp)
assert np.array_equal(platform.position, old_position)
platform.move(new_timestamp)
assert np.array_equal(platform.position, old_position)
platform_state = State(np.array([[2], [1], [2], [1], [0], [1]]), timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=None,
position_mapping=[0, 2, 4])
assert platform.is_moving
assert np.array_equal(platform.velocity, StateVector([1, 1, 1]))
old_position = platform.position
# check it doesn't move with timestamp = None
platform.move(None)
assert np.array_equal(platform.position, old_position)
with pytest.raises(AttributeError):
platform.move(timestamp)
# moving platform without velocity defined
platform_state = State(np.array([[2], [2], [0]]), timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=None,
position_mapping=[0, 2, 4])
with pytest.raises(AttributeError):
_ = platform.velocity
# pass in a velocity mapping
platform_state = State(np.array([[2], [1], [2], [1], [0], [1]]), timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=None,
position_mapping=[0, 2, 4],
velocity_mapping=velocity_mapping)
assert platform.is_moving
assert np.array_equal(platform.velocity, StateVector([1, 1, 1]))
old_position = platform.position
# check it doesn't move with timestamp = None
platform.move(None)
assert np.array_equal(platform.position, old_position)
with pytest.raises(AttributeError):
platform.move(timestamp)
# moving platform without velocity defined
platform_state = State(np.array([[2], [2], [0]]), timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=None,
position_mapping=[0, 2, 4],
velocity_mapping=velocity_mapping)
with pytest.raises(AttributeError):
_ = platform.velocity