Functions

Mathematical functions used within Stone Soup

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.jacobian(fun, x)

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

• list of length 2*Ns+1 – An list of States 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 (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

• 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.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_reduce_single(means, covars, weights)

Reduce mixture of multi-variate Gaussians to single Gaussian

Parameters

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

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

• weights (np.array 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