Repository: dstl/Stone-Soup

Issue:
Unscented Kalman Filter transform : Cannot cast ufunc 'add' output from dtype('float64') to dtype('int32') with casting rule 'same_kind'
I am trying to use UKF in place of EKF in the code fragment below. However for UKF i am getting the following stack trace (see below). I noted the defaults are set for UKF if no arguments are given. Am Am I missing setting up something for UKF?


```
from stonesoup.predictor.kalman import UnscentedKalmanPredictor
predictor = UnscentedKalmanPredictor(transition_model)
.
.
.
from stonesoup.updater.kalman import UnscentedKalmanUpdater
updater = UnscentedKalmanUpdater(measurement_model)


tracks = {Track([prior_one]), Track([prior_two])}
for n, (measurements, clutter_set) in enumerate(zip(measurementss, clutter), 1):
    detections = clutter_set.copy()
    detections.update(measurements) # Add measurements and clutter together
    
    hypotheses = data_associator.associate(tracks, detections, start_time+timedelta(seconds=n))
    for track in tracks:
        hypothesis = hypotheses[track]
        if hypothesis.measurement:
            post = updater.update(hypothesis)
            track.append(post)
        else: # When data associator says no detections are good enough, we'll keep the prediction
            track.append(hypothesis.prediction)
```



```
---------------------------------------------------------------------------
UFuncTypeError                            Traceback (most recent call last)
<ipython-input-23-75a62285695b> in <module>
      5     detections.update(measurements) # Add measurements and clutter together
      6 
----> 7     hypotheses = data_associator.associate(tracks, detections, start_time+timedelta(seconds=n))
      8     for track in tracks:
      9         hypothesis = hypotheses[track]

C:\g\mot\lib\a\lib\site-packages\stonesoup\dataassociator\neighbour.py in associate(self, tracks, detections, time)
    100         hypotheses = {
    101             track: self.hypothesiser.hypothesise(track, detections, time)
--> 102             for track in tracks}
    103 
    104         # Link hypotheses into a set of joint_hypotheses and evaluate

C:\g\mot\lib\a\lib\site-packages\stonesoup\dataassociator\neighbour.py in <dictcomp>(.0)
    100         hypotheses = {
    101             track: self.hypothesiser.hypothesise(track, detections, time)
--> 102             for track in tracks}
    103 
    104         # Link hypotheses into a set of joint_hypotheses and evaluate

C:\g\mot\lib\a\lib\site-packages\stonesoup\hypothesiser\distance.py in hypothesise(self, track, detections, timestamp)
     62 
     63         # Common state & measurement prediction
---> 64         prediction = self.predictor.predict(track.state, timestamp=timestamp)
     65         measurement_prediction = self.updater.get_measurement_prediction(
     66             prediction)

C:\g\mot\lib\a\lib\site-packages\stonesoup\predictor\kalman.py in predict(self, prior, control_input, timestamp, **kwargs)
    358             transition_noise_covar, control_input.state_vector,
    359             control_matrix, contol_noise_covar,
--> 360             self.alpha, self.beta, self.kappa)
    361 
    362         return GaussianStatePrediction(prediction_mean,

C:\g\mot\lib\a\lib\site-packages\stonesoup\predictor\kalman.py in predict_lowlevel(x, P, f, Q, u, B, Qu, alpha, beta, kappa)
    402 
    403         sigma_points, mean_weights, covar_weights = \
--> 404             gauss2sigma(x, P, alpha, beta, kappa)
    405 
    406         x_pred, P_pred, _, _, _, _ = unscented_transform(sigma_points,

C:\g\mot\lib\a\lib\site-packages\stonesoup\functions.py in gauss2sigma(mean, covar, alpha, beta, kappa)
    123     # Calculate sigma point locations
    124     sigma_points = np.tile(mean, (1, 2 * ndim_state + 1))
--> 125     sigma_points[:, 1:(ndim_state + 1)] += sqrt_sigma * np.sqrt(c)
    126     sigma_points[:, (ndim_state + 1):] -= sqrt_sigma * np.sqrt(c)
    127 

UFuncTypeError: Cannot cast ufunc 'add' output from dtype('float64') to dtype('int32') with casting rule 'same_kind'
```

Discussion:

Patch:
diff --git a/stonesoup/functions.py b/stonesoup/functions.py
index db7567f2..a1d971f3 100644
--- a/stonesoup/functions.py
+++ b/stonesoup/functions.py
@@ -130,8 +130,11 @@ 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))
-    sigma_points[:, 1:(ndim_state + 1)] += sqrt_sigma * np.sqrt(c)
-    sigma_points[:, (ndim_state + 1):] -= sqrt_sigma * np.sqrt(c)
+    # 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)
+    sigma_points[:, (ndim_state + 1):] = \
+        sigma_points[:, (ndim_state + 1):] - sqrt_sigma*np.sqrt(c)
 
     # Put these sigma points into s State object list
     sigma_points_states = []


Test patch:
diff --git a/stonesoup/tests/test_functions.py b/stonesoup/tests/test_functions.py
index ac5e135f..e82fe1d7 100644
--- a/stonesoup/tests/test_functions.py
+++ b/stonesoup/tests/test_functions.py
@@ -3,8 +3,8 @@ from numpy import deg2rad
 from pytest import approx
 
 from ..functions import (
-    jacobian, gm_reduce_single, mod_bearing, mod_elevation)
-from ..types.state import State
+    jacobian, gm_reduce_single, mod_bearing, mod_elevation, gauss2sigma)
+from ..types.state import State, GaussianState
 
 
 def test_jacobian():
@@ -109,3 +109,27 @@ def test_elevation():
 
     for ind, val in enumerate(rad_in):
         assert rad_out[ind] == approx(mod_elevation(val))
+
+
+def test_gauss2sigma_float():
+    mean = 1.0
+    covar = 2.0
+    state = GaussianState([[mean]], [[covar]])
+
+    sigma_points_states, mean_weights, covar_weights = gauss2sigma(state, kappa=0)
+
+    for n, sigma_point_state in zip((0, 1, -1), sigma_points_states):
+        assert sigma_point_state.state_vector[0, 0] == approx(mean + n*covar**0.5)
+
+
+def test_gauss2sigma_int():
+    mean = 1
+    covar = 2.0
+    state = GaussianState([[mean]], [[covar]])
+
+    sigma_points_states, mean_weights, covar_weights = gauss2sigma(state, kappa=0)
+
+    for n, sigma_point_state in zip((0, 1, -1), sigma_points_states):
+        # Resultant sigma points are still ints
+        assert sigma_point_state.state_vector[0, 0] == int(mean + n*covar**0.5)
+        assert isinstance(sigma_point_state.state_vector[0, 0], np.integer)