From a27b6eb49737eb1613bb705570269b59a43f6081 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Mon, 6 Apr 2020 12:40:10 +0100 Subject: [PATCH 1/5] Change StateVector indexing so that my_statevector[1] works as 'expected' and update code/tests to match --- stonesoup/feeder/geo.py | 6 ++--- stonesoup/feeder/tests/test_geo.py | 4 +-- stonesoup/functions.py | 2 ++ stonesoup/initiator/tests/test_simple.py | 2 +- stonesoup/models/measurement/nonlinear.py | 32 ++++++++++------------- stonesoup/sensor/radar/radar.py | 6 ++--- stonesoup/sensor/tests/test_passive.py | 2 +- stonesoup/types/array.py | 23 ++++++++++++++++ stonesoup/types/tests/test_array.py | 26 ++++++++++++++++++ 9 files changed, 75 insertions(+), 28 deletions(-) diff --git a/stonesoup/feeder/geo.py b/stonesoup/feeder/geo.py index e31e20b50..b6907b883 100644 --- a/stonesoup/feeder/geo.py +++ b/stonesoup/feeder/geo.py @@ -91,8 +91,8 @@ class LongLatToUTMConverter(Feeder): """ mapping = Property( - (float, float), default=(0, 1), - doc="Indexes of long, lat. Default (0, 1)") + (float, float), default=[0, 1], + doc="Indexes of long, lat. Default [0, 1]") zone_number = Property( int, default=None, doc="UTM zone number to carry out conversion. Default `None`, where it" @@ -109,7 +109,7 @@ def detections_gen(self): utm_detections = set() for detection in detections: easting, northing, zone_num, northern = utm.from_latlon( - *detection.state_vector[self.mapping[::-1], 0], + *detection.state_vector[self.mapping[::-1]], self.zone_number) if self.zone_number is None: self.zone_number = zone_num diff --git a/stonesoup/feeder/tests/test_geo.py b/stonesoup/feeder/tests/test_geo.py index 1ce77e732..a9596800c 100644 --- a/stonesoup/feeder/tests/test_geo.py +++ b/stonesoup/feeder/tests/test_geo.py @@ -35,7 +35,7 @@ def test_lla_reference_converter(detector, converter_class, reverse_func): detection = detections.pop() assert pytest.approx((50, i, 5000 + i*10), abs=1e-2, rel=1e-3) == \ - reverse_func(*detection.state_vector[:, 0], 50, 0, 5000) + reverse_func(*detection.state_vector, 50, 0, 5000) def test_utm_converter(detector): @@ -54,4 +54,4 @@ def test_utm_converter(detector): p_east = detection.state_vector[0] assert pytest.approx((50, long), rel=1e-2, abs=1e-4) == utm.to_latlon( - *detection.state_vector[0:2, 0], zone_number=30, northern=True) + *detection.state_vector[0:2], zone_number=30, northern=True) diff --git a/stonesoup/functions.py b/stonesoup/functions.py index 5a93349b9..b14c4f1e5 100644 --- a/stonesoup/functions.py +++ b/stonesoup/functions.py @@ -130,6 +130,8 @@ def gauss2sigma(state, alpha=1.0, beta=2.0, kappa=None): # Calculate sigma point locations sigma_points = np.tile(state.state_vector, (1, 2 * ndim_state + 1)) + # as sigma_points is a 2d it should no longer be a StateVector + sigma_points = Matrix(sigma_points) # Can't use in place addition/subtraction as casting issues may arise when mixing float/int sigma_points[:, 1:(ndim_state + 1)] = \ sigma_points[:, 1:(ndim_state + 1)] + sqrt_sigma*np.sqrt(c) diff --git a/stonesoup/initiator/tests/test_simple.py b/stonesoup/initiator/tests/test_simple.py index b8dc6ad71..fbddd1c0f 100644 --- a/stonesoup/initiator/tests/test_simple.py +++ b/stonesoup/initiator/tests/test_simple.py @@ -114,7 +114,7 @@ def test_linear_measurement(): def test_nonlinear_measurement(): measurement_model = CartesianToBearingRange( - 2, (0, 1), np.diag([np.radians(2), 30])) + 2, [0, 1], np.diag([np.radians(2), 30])) measurement_initiator = SimpleMeasurementInitiator( GaussianState(np.array([[0], [0]]), np.diag([100, 10])), measurement_model diff --git a/stonesoup/models/measurement/nonlinear.py b/stonesoup/models/measurement/nonlinear.py index d449022d7..8143bc3f6 100644 --- a/stonesoup/models/measurement/nonlinear.py +++ b/stonesoup/models/measurement/nonlinear.py @@ -244,23 +244,20 @@ def function(self, state, noise=False, **kwargs): xyz_rot = self._rotation_matrix @ xyz # Convert to Spherical - rho, phi, theta = cart2sphere(*xyz_rot[:, 0]) + rho, phi, theta = cart2sphere(*xyz_rot) return StateVector([[Elevation(theta)], [Bearing(phi)], [rho]]) + noise def inverse_function(self, detection, **kwargs): - theta, phi, rho = detection.state_vector[:, 0] - x, y, z = sphere2cart(rho, phi, theta) + theta, phi, rho = detection.state_vector + xyz = StateVector(sphere2cart(rho, phi, theta)) - xyz = [[x], [y], [z]] inv_rotation_matrix = inv(self._rotation_matrix) - xyz_rot = inv_rotation_matrix @ xyz - xyz = [xyz_rot[0][0], xyz_rot[1][0], xyz_rot[2][0]] - x, y, z = xyz + self.translation_offset[:, 0] + xyz = inv_rotation_matrix @ xyz res = np.zeros((self.ndim_state, 1)).view(StateVector) - res[self.mapping, 0] = x, y, z + res[self.mapping] = xyz + self.translation_offset return res @@ -343,23 +340,22 @@ def ndim_meas(self): return 2 def inverse_function(self, detection, **kwargs): - if not ((self.rotation_offset[0][0] == 0) - and (self.rotation_offset[1][0] == 0)): + if not ((self.rotation_offset[0] == 0) + and (self.rotation_offset[1] == 0)): raise RuntimeError( "Measurement model assumes 2D space. \ Rotation in 3D space is unsupported at this time.") - phi, rho = detection.state_vector[:, 0] - x, y = pol2cart(rho, phi) + phi, rho = detection.state_vector[:] + xy = StateVector(pol2cart(rho, phi)) - xyz = [[x], [y], [0]] + xyz = np.concatenate((xy, StateVector([0])), axis=0) inv_rotation_matrix = inv(self._rotation_matrix) - xyz_rot = inv_rotation_matrix @ xyz - xy = [xyz_rot[0][0], xyz_rot[1][0]] - x, y = xy + self.translation_offset[:, 0] + xyz = inv_rotation_matrix @ xyz + xy = xyz[0:2] res = np.zeros((self.ndim_state, 1)).view(StateVector) - res[self.mapping, 0] = x, y + res[self.mapping] = xy + self.translation_offset return res @@ -513,7 +509,7 @@ def function(self, state, noise=False, **kwargs): xyz_rot = self._rotation_matrix @ xyz # Convert to Angles - phi, theta = cart2angles(*xyz_rot[:, 0]) + phi, theta = cart2angles(*xyz_rot) return StateVector([[Elevation(theta)], [Bearing(phi)]]) + noise diff --git a/stonesoup/sensor/radar/radar.py b/stonesoup/sensor/radar/radar.py index fb4fff380..c23131d6d 100644 --- a/stonesoup/sensor/radar/radar.py +++ b/stonesoup/sensor/radar/radar.py @@ -455,13 +455,13 @@ def gen_probability(self, sky_state): [r, pos_az, pos_el] = cart2sphere(*relative_vector) # target position relative to beam position - relative_az = pos_az[0] - beam_az - relative_el = pos_el[0] - beam_el + relative_az = pos_az - beam_az + relative_el = pos_el - beam_el # calculate power directed towards target self.beam_shape.beam_width = spoiled_width # beam spoiling to width directed_power = self.beam_shape.beam_power(relative_az, relative_el) # calculate signal to noise ratio - snr = self._snr_constant * rcs * spoiled_gain ** 2 * directed_power / (r[0] ** 4) + snr = self._snr_constant * rcs * spoiled_gain ** 2 * directed_power / (r ** 4) # calculate probability of detection using the North's approximation det_prob = 0.5 * erfc( (-np.log(self.probability_false_alarm)) ** 0.5 - ( diff --git a/stonesoup/sensor/tests/test_passive.py b/stonesoup/sensor/tests/test_passive.py index 44b0375fb..f7dc7b63a 100644 --- a/stonesoup/sensor/tests/test_passive.py +++ b/stonesoup/sensor/tests/test_passive.py @@ -48,7 +48,7 @@ def test_passive_sensor(): xyz_rot = rot_mat @ xyz # Convert to Angles - phi, theta = cart2angles(*xyz_rot[:, 0]) + phi, theta = cart2angles(*xyz_rot) # Assert correction of generated measurement assert (measurement.timestamp == target_state.timestamp) diff --git a/stonesoup/types/array.py b/stonesoup/types/array.py index a520026fc..930135ae3 100644 --- a/stonesoup/types/array.py +++ b/stonesoup/types/array.py @@ -50,6 +50,17 @@ class StateVector(Matrix): ``StateVector([1., 2., 3.])``, ``StateVector ([[1., 2., 3.,]])``, and ``StateVector([[1.], [2.], [3.]])`` will all return the same 3x1 StateVector. + It also overrides the behaviour of indexing such that my_state_vector[1] returns the second + element (as `int`, `float` etc), rather than a StateVector of size (1, 1) as would be the case + without this override. Behaviour of indexing with lists, slices or other indexing is + unaffected (as you would expect those to return StateVectors). This override avoids the need + for client to specifically index with zero as the second element (`my_state_vector[1, 0]`) to + get a native numeric type. Iterating through the StateVector returns a sequence of numbers, + rather than a sequence of 1x1 StateVectors. This makes the class behave as would be expected + and avoids 'gotchas'. + + Note that code using the pattern `my_state_vector[1, 0]` will continue to work. + .. note :: It is not recommended to use a StateVector for indexing another vector. Doing so will lead to unexpected effects. Use a :class:`tuple`, :class:`list` or :class:`np.ndarray` for this. @@ -70,6 +81,18 @@ def __new__(cls, *args, **kwargs): array.shape)) return array.view(cls) + def __getitem__(self, item): + # If item has two elements, it is a tuple and should be left alone. + # If item is a slice object, or an ndarray, we would expect a StateVector returned, + # so leave it alone. + # If item is an int, we would expected a number returned, so we should append 0 to the + # item and extract the first (and only) column + # Note that an ndarray of ints is an instance of int + # i.e. isinstance(np.array([1]), int) == True + if isinstance(item, int): # and not isinstance(item, np.ndarray): + item = (item, 0) + return super().__getitem__(item) + class CovarianceMatrix(Matrix): """Covariance matrix wrapper for :class:`numpy.ndarray`. diff --git a/stonesoup/types/tests/test_array.py b/stonesoup/types/tests/test_array.py index a42d7f07e..fd945929c 100644 --- a/stonesoup/types/tests/test_array.py +++ b/stonesoup/types/tests/test_array.py @@ -18,6 +18,32 @@ def test_statevector(): assert np.array_equal(state_vector, state_vector_array) assert np.array_equal(StateVector([1, 2, 3, 4]), state_vector_array) assert np.array_equal(StateVector([[1, 2, 3, 4]]), state_vector_array) + assert np.array_equal(StateVector(state_vector_array), state_vector) + + +def test_standard_statevector_indexing(): + state_vector_array = np.array([[1], [2], [3], [4]]) + state_vector = StateVector(state_vector_array) + + # test standard indexing + assert state_vector[2, 0] == 3 + assert not isinstance(state_vector[2, 0], StateVector) + + # test Slicing + assert state_vector[1:2, 0] == 2 + assert isinstance(state_vector[1:2, 0], StateVector) + assert np.array_equal(state_vector[:], state_vector) + assert isinstance(state_vector[:, 0], StateVector) + assert np.array_equal(state_vector[0:], state_vector) + assert isinstance(state_vector[0:, 0], StateVector) + + # test list indices + assert np.array_equal(state_vector[[1, 3]], StateVector([2, 4])) + assert isinstance(state_vector[[1, 3], 0], StateVector) + + # test int indexing + assert state_vector[2] == 3 + assert not isinstance(state_vector[2], StateVector) def test_covariancematrix(): From 7b903a7c64364ed2cd23306b61311c40d82fe0d5 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Wed, 8 Apr 2020 10:50:44 +0100 Subject: [PATCH 2/5] Update StateVector __setitem__ to match __getitem__ --- stonesoup/types/array.py | 7 ++++++- stonesoup/types/tests/test_array.py | 17 +++++++++++++++++ 2 files changed, 23 insertions(+), 1 deletion(-) diff --git a/stonesoup/types/array.py b/stonesoup/types/array.py index 930135ae3..d9c899791 100644 --- a/stonesoup/types/array.py +++ b/stonesoup/types/array.py @@ -89,10 +89,15 @@ def __getitem__(self, item): # item and extract the first (and only) column # Note that an ndarray of ints is an instance of int # i.e. isinstance(np.array([1]), int) == True - if isinstance(item, int): # and not isinstance(item, np.ndarray): + if isinstance(item, int): item = (item, 0) return super().__getitem__(item) + def __setitem__(self, key, value): + if isinstance(key, int): + key = (key, 0) + return super().__setitem__(key, value) + class CovarianceMatrix(Matrix): """Covariance matrix wrapper for :class:`numpy.ndarray`. diff --git a/stonesoup/types/tests/test_array.py b/stonesoup/types/tests/test_array.py index fd945929c..d922e5f43 100644 --- a/stonesoup/types/tests/test_array.py +++ b/stonesoup/types/tests/test_array.py @@ -46,6 +46,23 @@ def test_standard_statevector_indexing(): assert not isinstance(state_vector[2], StateVector) +def test_setting(): + state_vector_array = np.array([[1], [2], [3], [4]]) + state_vector = StateVector(state_vector_array.copy()) + + state_vector[2, 0] = 4 + assert np.array_equal(state_vector, StateVector([1, 2, 4, 4])) + + state_vector[2] = 5 + assert np.array_equal(state_vector, StateVector([1, 2, 5, 4])) + + state_vector[:] = state_vector_array[:] + assert np.array_equal(state_vector, StateVector([1, 2, 3, 4])) + + state_vector[1:3] = StateVector([5, 6]) + assert np.array_equal(state_vector, StateVector([1, 5, 6, 4])) + + def test_covariancematrix(): """ CovarianceMatrix Type test """ From 01a44ae30f49eda55fb89275e2b4ea5ef2bef34d Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Tue, 28 Apr 2020 15:09:46 +0100 Subject: [PATCH 3/5] Fix test in simulator --- stonesoup/simulator/tests/test_platform.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/stonesoup/simulator/tests/test_platform.py b/stonesoup/simulator/tests/test_platform.py index 86950fd6d..b3c6c266e 100644 --- a/stonesoup/simulator/tests/test_platform.py +++ b/stonesoup/simulator/tests/test_platform.py @@ -85,7 +85,7 @@ def test_platform_ground_truth_detection_simulator(sensor_model1, for detection in detections: for i in range(0, len(detection.state_vector)): # Detection at location of ground truth. - assert int(detection.state_vector[i][0]) == int(n/2) + assert int(detection.state_vector[i]) == int(n/2) def test_detection_simulator(sensor_model1, @@ -108,4 +108,4 @@ def test_detection_simulator(sensor_model1, assert len(detections) == 2 # Detection count at each step. for detection in detections: # Detection at location of ground truth or at platform. - assert int(detection.state_vector[0][0]) in (int(n/3), 2*int(n/3)) + assert int(detection.state_vector[0]) in (int(n/3), 2*int(n/3)) From 5478aaa5309218cd0aa681f541f2a3aec7686ba7 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Tue, 28 Apr 2020 15:13:33 +0100 Subject: [PATCH 4/5] Update mapping type in LongLatToUTMConverter --- stonesoup/feeder/geo.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stonesoup/feeder/geo.py b/stonesoup/feeder/geo.py index b6907b883..f30a4c9b3 100644 --- a/stonesoup/feeder/geo.py +++ b/stonesoup/feeder/geo.py @@ -91,7 +91,7 @@ class LongLatToUTMConverter(Feeder): """ mapping = Property( - (float, float), default=[0, 1], + [float, float], default=[0, 1], doc="Indexes of long, lat. Default [0, 1]") zone_number = Property( int, default=None, From b52a2736ae7c7c9a2068522dc52ef360846b72ca Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Tue, 5 May 2020 12:33:16 +0100 Subject: [PATCH 5/5] Remove a mutable default and correct mapping usage to allow tuple mappings --- stonesoup/feeder/geo.py | 6 +++--- stonesoup/models/measurement/nonlinear.py | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/stonesoup/feeder/geo.py b/stonesoup/feeder/geo.py index f30a4c9b3..a719e89b1 100644 --- a/stonesoup/feeder/geo.py +++ b/stonesoup/feeder/geo.py @@ -91,8 +91,8 @@ class LongLatToUTMConverter(Feeder): """ mapping = Property( - [float, float], default=[0, 1], - doc="Indexes of long, lat. Default [0, 1]") + (float, float), default=(0, 1), + doc="Indexes of long, lat. Default (0, 1)") zone_number = Property( int, default=None, doc="UTM zone number to carry out conversion. Default `None`, where it" @@ -109,7 +109,7 @@ def detections_gen(self): utm_detections = set() for detection in detections: easting, northing, zone_num, northern = utm.from_latlon( - *detection.state_vector[self.mapping[::-1]], + *detection.state_vector[self.mapping[::-1], :], self.zone_number) if self.zone_number is None: self.zone_number = zone_num diff --git a/stonesoup/models/measurement/nonlinear.py b/stonesoup/models/measurement/nonlinear.py index 8143bc3f6..7431b4cad 100644 --- a/stonesoup/models/measurement/nonlinear.py +++ b/stonesoup/models/measurement/nonlinear.py @@ -257,7 +257,7 @@ def inverse_function(self, detection, **kwargs): xyz = inv_rotation_matrix @ xyz res = np.zeros((self.ndim_state, 1)).view(StateVector) - res[self.mapping] = xyz + self.translation_offset + res[self.mapping, :] = xyz + self.translation_offset return res @@ -355,7 +355,7 @@ def inverse_function(self, detection, **kwargs): xy = xyz[0:2] res = np.zeros((self.ndim_state, 1)).view(StateVector) - res[self.mapping] = xy + self.translation_offset + res[self.mapping, :] = xy + self.translation_offset return res @@ -503,7 +503,7 @@ def function(self, state, noise=False, **kwargs): noise = 0 # Account for origin offset - xyz = state.state_vector[self.mapping] - self.translation_offset + xyz = state.state_vector[self.mapping, :] - self.translation_offset # Rotate coordinates xyz_rot = self._rotation_matrix @ xyz