Functions

Mathematical functions used within Stone Soup

stonesoup.functions.block_diag(*arrays)

Create a block diagonal matrix from the provided 2-D (or lower) arrays.

A lightweight, drop-in replacement for scipy.linalg.block_diag() for the plain numpy.ndarray inputs Stone Soup calls it with (no batching, no array-API dispatch), which is substantially faster for the small matrices typical of combined transition/measurement models.

Parameters:

*arrays (numpy.ndarray) – Input arrays, treated as 2-D (scalar and 1-D inputs are promoted, as per numpy.atleast_2d()).

Returns:

Array with the inputs arranged on the diagonal.

Return type:

numpy.ndarray

stonesoup.functions.grid_creation(xp_aux, Pp_aux, sFactor, nx, Npa)

Grid for point mass filter

Create a PMF grid based on center, covariance matrix, and sigma probability

Parameters:

• xp_aux (numpy.ndarray) – nx by 1 center of the grid

• Pp_aux (numpy.ndarray) – ‘nx’ by ‘nx’ covariance matrix

• sFactor (int) – Parameter for the size of the grid

• nx (int) – Dimension of the grid

• Npa (numpy.ndarray) – ‘nx’ by ‘’ number of points per axis of the grid

Returns:

• predGrid (numpy.ndarray) – ‘nx’ by prod(Npa) predictive grid

• predGridDelta (list) – grid step per dimension

• gridDim (list of numpy.ndarrays) – grid coordinates per dimension before rotation and translation

• xp_aux (numpy.ndarray) – grid center

• eigVect (numpy.ndarray) – eigenvectors describing the rotation of the grid

stonesoup.functions.tria(matrix)

Square Root Matrix Triangularization

Given a rectangular square root matrix obtain a square lower-triangular square root matrix

Parameters:

matrix (numpy.ndarray) – A n by m matrix that is generally not square.

Returns:

A square lower-triangular matrix.

Return type:

numpy.ndarray

stonesoup.functions.cholesky_eps(A, lower=False)

Perform a Cholesky decomposition on a nearly positive-definite matrix.

This should return similar results to NumPy/SciPy Cholesky decompositions, but compromises for cases for non positive-definite matrix.

Parameters:

• A (numpy.ndarray) – Symmetric positive-definite matrix.

• lower (bool) – Whether to return lower or upper triangular decomposition. Default False which returns upper.

Returns:

L – Upper/lower triangular Cholesky decomposition.

Return type:

numpy.ndarray

stonesoup.functions.jacobian(fun, x, **kwargs)

Compute Jacobian through finite difference calculation

Parameters:

• fun (function handle) – A (non-linear) transition function Must be of the form “y = fun(x)”, where y can be a scalar or numpy.ndarray of shape (Nd, 1) or (Nd,)

• x (State) – A state with state vector of shape (Ns, 1)

Returns:

jac – The computed Jacobian

Return type:

numpy.ndarray of shape (Nd, Ns)

stonesoup.functions.gauss2sigma(state, alpha=1.0, beta=2.0, kappa=None)

Approximate a given distribution to a Gaussian, using a deterministically selected set of sigma points.

Parameters:

• state (State) – A state object capable of returning a StateVector of shape (Ns, 1) representing the Gaussian mean and a CovarianceMatrix of shape (Ns, Ns) which is the covariance of the distribution

• alpha (float, optional) – Spread of the sigma points. Typically 1e-3. (default is 1)

• beta (float, optional) – Used to incorporate prior knowledge of the distribution 2 is optimal if the state is normally distributed. (default is 2)

• kappa (float, optional) – Secondary spread scaling parameter (default is calculated as 3-Ns)

Returns:

• State with state vector of shape (Ns, 2*Ns+1) – An State containing the locations of the sigma points. Note that only the state_vector attribute in these States will be meaningful. Other quantities, like covar will be inherited from the input and don’t really make sense for a sigma point.

• numpy.ndarray of shape (2*Ns+1,) – An array containing the sigma point mean weights

• numpy.ndarray of shape (2*Ns+1,) – An array containing the sigma point covariance weights

stonesoup.functions.sigma2gauss(sigma_points, mean_weights, covar_weights, covar_noise=None)

Calculate estimated mean and covariance from a given set of sigma points

Parameters:

• sigma_points (StateVectors of shape (Ns, 2*Ns+1)) – An array containing the locations of the sigma points

• mean_weights (numpy.ndarray of shape (2*Ns+1,)) – An array containing the sigma point mean weights

• covar_weights (numpy.ndarray of shape (2*Ns+1,)) – An array containing the sigma point covariance weights

• covar_noise (CovarianceMatrix of shape (Ns, Ns), optional) – Additive noise covariance matrix (default is None)

Returns:

• StateVector of shape (Ns, 1) – Calculated mean

• CovarianceMatrix of shape (Ns, Ns) – Calculated covariance

stonesoup.functions.unscented_transform(sigma_points_states, mean_weights, covar_weights, fun, points_noise=None, covar_noise=None)

Apply the Unscented Transform to a set of sigma points

Apply f to points (with secondary argument points_noise, if available), then approximate the resulting mean and covariance. If sigma_noise is available, treat it as additional variance due to additive noise.

Parameters:

• sigma_points_states (State with state vector of shape (Ns, 2*Ns+1)) – A state containing the locations of the sigma points

• mean_weights (numpy.ndarray of shape (2*Ns+1,)) – An array containing the sigma point mean weights

• covar_weights (numpy.ndarray of shape (2*Ns+1,)) – An array containing the sigma point covariance weights

• fun (function handle) – A (non-linear) transition function Must be of the form “y = fun(x,w)”, where y can be a scalar or numpy.ndarray of shape (Ns, 1) or (Ns,)

• covar_noise (CovarianceMatrix of shape (Ns, Ns), optional) – Additive noise covariance matrix (default is None)

• points_noise (numpy.ndarray of shape (Ns, 2*Ns+1,), optional) – points to pass into f’s second argument (default is None)

Returns:

• StateVector of shape (Ns, 1) – Transformed mean

• CovarianceMatrix of shape (Ns, Ns) – Transformed covariance

• CovarianceMatrix of shape (Ns,Nm) – Calculated cross-covariance matrix

• StateVectors of shape (Ns, 2*Ns+1) – An array containing the locations of the transformed sigma points

• numpy.ndarray of shape (2*Ns+1,) – An array containing the transformed sigma point mean weights

• numpy.ndarray of shape (2*Ns+1,) – An array containing the transformed sigma point covariance weights

stonesoup.functions.cart2pol(x, y)

Convert Cartesian coordinates to Polar

Parameters:

• x (float) – The x coordinate

• y (float) – the y coordinate

Returns:

A tuple of the form (range, bearing)

Return type:

(float, float)

stonesoup.functions.cart2sphere(x, y, z)

Convert Cartesian coordinates to Spherical

Parameters:

• x (float) – The x coordinate

• y (float) – the y coordinate

• z (float) – the z coordinate

Returns:

A tuple of the form (range, bearing, elevation) bearing and elevation in radians. Elevation is measured from x, y plane

Return type:

(float, float, float)

stonesoup.functions.cart2angles(x, y, z)

Convert Cartesian coordinates to Angles

Parameters:

• x (float) – The x coordinate

• y (float) – the y coordinate

• z (float) – the z coordinate

Returns:

A tuple of the form (bearing, elevation) bearing and elevation in radians. Elevation is measured from x, y plane

Return type:

(float, float)

stonesoup.functions.pol2cart(rho, phi)

Convert Polar coordinates to Cartesian

Parameters:

• rho (float) – Range(a.k.a. radial distance)

• phi (float) – Bearing, expressed in radians

Returns:

A tuple of the form (x, y)

Return type:

(float, float)

stonesoup.functions.sphere2cart(rho, phi, theta)

Convert Polar coordinates to Cartesian

Parameters:

• rho (float) – Range(a.k.a. radial distance)

• phi (float) – Bearing, expressed in radians

• theta (float) – Elevation expressed in radians, measured from x, y plane

Returns:

A tuple of the form (x, y, z)

Return type:

(float, float, float)

stonesoup.functions.cart2az_el_rg(x, y, z)

Convert Cartesian to azimuth (phi), elevation(theta), and range(rho)

Parameters:

• x (float) – The x coordinate

• y (float) – the y coordinate

• z (float) – the z coordinate

Returns:

A tuple of the form (phi, theta, rho)

Return type:

(float, float, float)

stonesoup.functions.az_el_rg2cart(phi, theta, rho)

Convert azimuth (phi), elevation(theta), and range(rho) to Cartesian

Parameters:

• phi (float) – azimuth, expressed in radians

• theta (float) – Elevation expressed in radians, measured from x, y plane

• rho (float) – Range(a.k.a. radial distance)

Returns:

A tuple of the form (phi, theta, rho)

Return type:

(float, float, float)

stonesoup.functions.rotx(theta)

Rotation matrix for rotations around x-axis

For a given rotation angle: \(\theta\), this function evaluates and returns the rotation matrix:

(1)\[\begin{split}R_{x}(\theta) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & cos(\theta) & -sin(\theta) \\ 0 & sin(\theta) & cos(\theta) \end{bmatrix}\end{split}\]

Parameters:

theta (Union[float, np.ndarray]) – Rotation angle specified as a real-valued number or an np.ndarray of reals. The rotation angle is positive if the rotation is in the clockwise direction when viewed by an observer looking down the x-axis towards the origin. Angle units are in radians.

Returns:

Rotation matrix around x-axis of the form (1).

Return type:

numpy.ndarray of shape (3, 3) or (3, 3, n) for array input

stonesoup.functions.roty(theta)

Rotation matrix for rotations around y-axis

For a given rotation angle: \(\theta\), this function evaluates and returns the rotation matrix:

(2)\[\begin{split}R_{y}(\theta) = \begin{bmatrix} cos(\theta) & 0 & sin(\theta) \\ 0 & 1 & 0 \\ - sin(\theta) & 0 & cos(\theta) \end{bmatrix}\end{split}\]

Parameters:

theta (Union[float, np.ndarray]) – Rotation angle specified as a real-valued number or an np.ndarray of reals. The rotation angle is positive if the rotation is in the clockwise direction when viewed by an observer looking down the y-axis towards the origin. Angle units are in radians.

Returns:

Rotation matrix around y-axis of the form (2).

Return type:

numpy.ndarray of shape (3, 3) or (3, 3, n) for array input

stonesoup.functions.rotz(theta)

Rotation matrix for rotations around z-axis

For a given rotation angle: \(\theta\), this function evaluates and returns the rotation matrix:

(3)\[\begin{split}R_{z}(\theta) = \begin{bmatrix} cos(\theta) & -sin(\theta) & 0 \\ sin(\theta) & cos(\theta) & 0 \\ 0 & 0 & 1 \end{bmatrix}\end{split}\]

Parameters:

theta (Union[float, np.ndarray]) – Rotation angle specified as a real-valued number or an np.ndarray of reals. The rotation angle is positive if the rotation is in the clockwise direction when viewed by an observer looking down the z-axis towards the origin. Angle units are in radians.

Returns:

Rotation matrix around z-axis of the form (3).

Return type:

numpy.ndarray of shape (3, 3) or (3, 3, n) for array input

stonesoup.functions.gm_sample(means, covars, size, weights=None, random_state=None)

Sample from a mixture of multi-variate Gaussians

Parameters:

• means (StateVector, StateVectors, np.ndarray of shape (num_dims, num_components)) – The means of GM components

• covars (np.ndarray of shape (num_components, num_dims, num_dims) or list of np.ndarray of shape (num_dims, num_dims)) – Covariance matrices of the GM components

• size (int) – Number of samples to return.

• weights (np.ndarray of shape (num_components, ), optional) – The weights of the GM components. If not defined, assumed equal.

Return type:

StateVectors of shape (num_dims, size)

stonesoup.functions.gm_reduce_single(means, covars, weights)

Reduce mixture of multi-variate Gaussians to single Gaussian

Parameters:

• means (StateVectors) – The means of the GM components

• covars (numpy.ndarray of shape (num_dims, num_dims, num_components)) – The covariance matrices of the GM components

• weights (numpy.ndarray of shape (num_components,)) – The weights of the GM components

Returns:

• StateVector – The mean of the reduced/single Gaussian

• CovarianceMatrix – The covariance of the reduced/single Gaussian

stonesoup.functions.mod_bearing(x)

Calculates the modulus of a bearing. Bearing angles are within the range \(-\pi\) to \(\pi\).

Parameters:

x (float) – bearing angle in radians

Returns:

Angle in radians in the range math: \(-\pi\) to \(+\pi\)

Return type:

float

stonesoup.functions.mod_elevation(x)

Calculates the modulus of an elevation angle. Elevation angles are within the range \(-\pi/2\) to \(\pi/2\).

Parameters:

x (float) – elevation angle in radians

Returns:

Angle in radians in the range math: \(-\pi/2\) to \(+\pi/2\)

Return type:

float

Note

Uses the identity \(\arcsin(\sin(x))\), which folds any angle into \([-\pi/2, \pi/2]\) with the same symmetry as elevation wrapping.

stonesoup.functions.build_rotation_matrix(angle_vector: ndarray)

Calculates and returns the (3D) axis rotation matrix given a vector of three angles: [roll, pitch/elevation, yaw/azimuth] Order of rotations is in reverse: yaw, pitch, roll (z, y, x) This is the rotation matrix that implements the rotations that convert the input angle_vector to match the x-axis.

Parameters:

angle_vector (numpy.ndarray of shape (3, 1)) – the rotations about the :math:’x, y, z’ axes. In aircraft/radar terms these correspond to [roll, pitch/elevation, yaw/azimuth]

Returns:

The model (3D) rotation matrix.

Return type:

numpy.ndarray of shape (3, 3)

stonesoup.functions.build_rotation_matrix_xyz(angle_vector: ndarray)

Calculates and returns the (3D) axis rotation matrix given a vector of three angles: [roll, pitch/elevation, yaw/azimuth] Order of rotations is roll, pitch, yaw (x, y, z) This is the rotation matrix that implements the rotations that convert a vector aligned to the x-axis to the input angle_vector.

Parameters:

angle_vector (numpy.ndarray of shape (3, 1)) – the rotations about the :math:’x, y, z’ axes. In aircraft/radar terms these correspond to [roll, pitch/elevation, yaw/azimuth]

Returns:

The model (3D) rotation matrix.

Return type:

numpy.ndarray of shape (3, 3)

stonesoup.functions.dotproduct(a, b)

Returns the dot (or scalar) product of two StateVectors or two sets of StateVectors.

The result for vectors of length \(n\) is \(\Sigma_i^n a_i b_i\).

Parameters:

• a (StateVector, StateVectors) – A (set of) state vector(s)

• b (StateVector, StateVectors) – A state vector(s) object of equal dimension to \(a\)

Returns:

A (set of) scalar value(s) representing the dot product of the vectors.

Return type:

float, numpy.array

stonesoup.functions.sde_euler_maruyama_integration(fun, t_values, state_x0)

Perform SDE Euler Maruyama Integration

Performs Stochastic Differential Equation Integration using the Euler Maruyama method.

Parameters:

• fun (callable) – Function to integrate.

• t_values (list of float) – Time values to integrate over

• state_x0 (State) – Initial state for time in first value in t_values.

Returns:

Final value for the time in last value in t_values

Return type:

StateVector

stonesoup.functions.gauss2cubature(state, alpha=1.0)

Evaluate the cubature points for an input Gaussian state. This is done under the assumption that the input state is \(\mathcal{N}(\mathbf{\mu}, \Sigma)\) of dimension \(n\). We calculate the square root of the covariance (via Cholesky factorization), and find the cubature points, \(X\), as,

\[ \begin{align}\begin{aligned}\Sigma &= S S^T\\X_i &= S \xi_i + \mathbf{\mu}\end{aligned}\end{align} \]

for \(i = 1,...,2n\), where \(\xi_i = \sqrt{ \alpha n} [\pm \mathbf{1}]_i\) and \([\pm \mathbf{1}]_i\) are the positive and negative unit vectors in each dimension. We include a scaling parameter \(\alpha\) to allow the selection of cubature points closer to the mean or more in the tails, as a potentially useful free parameter.

Parameters:

• state (GaussianState) – A Gaussian state with mean and covariance

• alpha (float, optional) – scaling parameter allowing the selection of cubature points closer to the mean (lower values) or further from the mean (higher values)

Returns:

Cubature points (as a StateVectors of dimension \(n \times 2n\))

Return type:

StateVectors

stonesoup.functions.cubature2gauss(cubature_points, covar_noise=None, alpha=1.0)

Get the predicted Gaussian mean and covariance from the cubature points. For dimension \(n\) there are \(m = 2n\) cubature points. The mean is,

\[\mu = \frac{1}{m} \sum\limits_{i=1}^{m} X_i\]

and the covariance

\[\Sigma = \frac{1}{\alpha}\left(\frac{1}{m} \sum\limits_{i=1}^{m} X_i X_i^T - \mathbf{\mu}\mathbf{\mu}^T\right) + Q\]

where \(Q\) is an optional additive noise matrix. The scaling parameter \(\alpha\) allow the for cubature points closer to the mean or more in the tails,

Parameters:

• cubature_points (StateVectors) – Cubature points (as a StateVectors of dimension \(n \times 2n\))

• covar_noise (CovarianceMatrix of shape (Ns, Ns), optional) – Additive noise covariance matrix (default is None)

• alpha (float, optional) – scaling parameter allowing the nomination of cubature points closer to the mean (lower values) or further from the mean (higher values)

Returns:

A Gaussian state with mean and covariance

Return type:

GaussianState

stonesoup.functions.cubature_transform(state, fun, points_noise=None, covar_noise=None, alpha=1.0)

Undertakes the cubature transform as described in [1]

Given a Gaussian distribution, calculates the set of cubature points using gauss2cubature(), then passes these through the given function and reconstructs the Gaussian using cubature2gauss(). Returns the mean, covariance, cross covariance and transformed cubature points. This instance includes a scaling parameter \(\alpha\), not included in the reference detailed above, which allows for the selection of cubature points closer to, or further from, tne mean.

Parameters:

• state (GaussianState) – A Gaussian state with mean and covariance

• fun (function handle) – A (non-linear) transition function Must be of the form “y = fun(x,w)”, where y can be a scalar or numpy.ndarray of shape (Ns, 1) or (Ns,)

• covar_noise (CovarianceMatrix of shape (Ns, Ns), optional) – Additive noise covariance matrix (default is None)

• points_noise (numpy.ndarray of shape (Ns, 2*Ns+1,), optional) – points to pass into f’s second argument (default is None)

• alpha (float, optional) – scaling parameter allowing the selection of cubature points closer to the mean (lower values) or further from the mean (higher values)

Returns:

• StateVector of shape (Ns, 1) – Transformed mean

• CovarianceMatrix of shape (Ns, Ns) – Transformed covariance

• CovarianceMatrix of shape (Ns,Nm) – Calculated cross-covariance matrix

• StateVectors of shape (Ns, 2*Ns) – An array containing the locations of the transformed cubature points

References

[1]

I. Arasaratnam and S. Haykin, “Cubature Kalman Filters,” in IEEE Transactions on Automatic Control, vol. 54, no. 6, pp. 1254-1269, June 2009, doi: 10.1109/TAC.2009.2019800.

stonesoup.functions.stochastic_cubature_rule_points(nx, order)

Computation of cubature points and weights for the stochastic integration.

Parameters:

• nx (int) – Number of points, presumably equivalent to state dimension.

• order (int) – Order for stochastic integration. Only orders 1, 3, and 5 are supported.

Returns:

Tuple of sigma points and weights

Return type:

(numpy.ndarray, numpy.ndarray)

stonesoup.functions.cub_points_and_tf(nx, order, sqrtCov, mean, transFunct, state)

Calculates cubature points for stochastic integration filter and puts them through given function (measurement/dynamics)

Parameters:

• nx (int) – Dimension for cubature points, equivalent to state dimension.

• order (int) – Order for Stochastic Integration. Only orders 1, 3, and 5 are supported

• sqrtCov (np.ndarray) – Matrix square root array of shape (nx, nx) of the covariance matrix

• mean (np.ndarray) – An array of shape (nx, 1) of the state mean

• transFunct (Callable) – A function to transfer state vectors

• state (State) – State object used to save function output to

Returns:

• points (numpy.ndarray) – Array of shape (nx, number of points) of cubature points

• w (numpy.ndarray) – Array of shape (number of points) of weights

• trsfPoints (numpy.ndarray) – Array of shape (nx, number of points) (based on order and dim) of cubature transformed points

stonesoup.functions.find_nearest_positive_definite(matrix, max_iterations=20)

Find the nearest positive definite matrix usable for cholesky factorization.

The nearest measure is in terms of Frobenius norm.

Parameters:

matrix (numpy.ndarray) – Base matrix to find the nearest positive definite from

Returns:

The nearest positive definite matrix

Return type:

numpy.ndarray

stonesoup.functions.is_cholesky_decomposable(matrix)

Test if given matrix is usable for cholesky decomposition.

Parameters:

matrix (numpy.ndarray) – Matrix to test

Returns:

An indication whether given matrix is usable for chelesky decomposition

Return type:

bool

stonesoup.functions.batch_multivariate_normal_logpdf(vectors, states)

Vectorised calculation for the multi-variate normal logpdf of N StateVector objects to N GaussianState objects.

The logpdf of vectors[m] will be calculated from the distribution states[m].

Parameters:

• vectors (list of StateVector) – Sequence of N StateVector objects.

• states (list of GaussianState) – Sequence of N GaussianState objects.

Returns:

Numpy array of shape (N,) of logpdf values.

Return type:

numpy.ndarray

Interpolation

stonesoup.functions.interpolate.time_range(start_time: datetime, end_time: datetime, timestep: timedelta = datetime.timedelta(seconds=1)) → Iterable[datetime]

Produces a range of datetime object between start_time (inclusive) and end_time (inclusive)

Parameters:

• start_time (datetime.datetime time range start (inclusive))

• end_time (datetime.datetime time range end (inclusive))

• timestep (datetime.timedelta default value is 1 second)

Return type:

Generator[datetime.datetime]

stonesoup.functions.interpolate.interpolate_state_mutable_sequence(sms: StateMutableSequence, times: datetime | list[datetime]) → StateMutableSequence | State

This function performs linear interpolation on a StateMutableSequence. The function has two slightly different forms:

If an individual datetime is inputted for the variable times then a State is returned corresponding to times.

If a list of datetime is inputted for the variable times then a StateMutableSequence is returned with the states in the sequence corresponding to times.

When interpolating the previous state is used to create the interpolated state. This means properties from that previous state are also copied but will not be interpolated e.g. covariance.

Parameters:

• sms (StateMutableSequence) – A StateMutableSequence that should be interpolated

• times (Union[datetime.datetime, list[datetime.datetime]]) – a time, or a list of times for sms to be interpolated to.

Returns:

If a single time is provided then a single state is returned. If a list of times is provided then a StateMutableSequence with the same type as sms is returned

Return type:

Union[StateMutableSequence, State]

Note

This function does not extrapolate. Times outside the range of the time range of sms are discarded and warning is given. If all times values are outside the time range of sms then an IndexError is raised.

Unique states for each time are required for interpolation. If there are multiple states with the same time in sms the later state in the sequence is used.

For Track inputs the metadatas is removed as it can’t be interpolated.