# Functions

Mathematical functions used within Stone Soup

stonesoup.functions.tria(matrix)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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

Parameters:
Returns:

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

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:
Returns:

stonesoup.functions.cart2pol(x, y)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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:

stonesoup.functions.mod_bearing(x)[source]

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)[source]

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

stonesoup.functions.build_rotation_matrix(angle_vector: ndarray)[source]

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)

• to (In aircraft/radar terms these correspond)

• [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)[source]

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)

• to (In aircraft/radar terms these correspond)

• [roll

• pitch/elevation

• yaw/azimuth]

Returns:

The model (3D) rotation matrix.

Return type:

numpy.ndarray of shape (3, 3)

stonesoup.functions.dotproduct(a, b)[source]

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:
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)[source]

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)[source]

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)[source]

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:
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)[source]

Undertakes the cubature transform as described in [3]

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:

References

## Orbital

### Orbital functions

Functions used within multiple orbital classes in Stone Soup

stonesoup.functions.orbital.stumpff_s(z)[source]

The Stumpff S function

$\begin{split}S(z) = \begin{cases}\frac{\sqrt(z) - \sin{\sqrt(z)}}{(\sqrt(z))^{3}}, & (z > 0)\\ \frac{\sinh(\sqrt(-z)) - \sqrt(-z)}{(\sqrt(-z))^{3}}, & (z < 0) \\ \frac{1}{6}, & (z = 0)\end{cases}\end{split}$
Parameters:

z (float, array-like) – input parameter, $$z$$ or $$[z]$$

Returns:

Output value, $$S(z)$$ in the same format and same size as input.

Return type:

float, array-like

stonesoup.functions.orbital.stumpff_c(z)[source]

The Stumpff C function

$\begin{split}C(z) = \begin{cases}\frac{1 - \cos{\sqrt(z)}}{z}, & (z > 0)\\ \frac{\cosh{\sqrt(-z)} - 1}{-z}, & (z < 0) \\ \frac{1}{2}, & (z = 0)\end{cases}\end{split}$
Parameters:

z (float, array-like) – input parameter, $$z$$

Returns:

Output value, $$C(z)$$ in same format and size as input

Return type:

float, array-like

stonesoup.functions.orbital.universal_anomaly_newton(o_state_vector, delta_t, grav_parameter=398600441800000.0, precision=1e-08, max_iterations=100000.0)[source]

Calculate the universal anomaly via Newton’s method. Algorithm 3.3 in [1].

Parameters:
• o_state_vector (StateVector, StateVectors) – The orbital state vector formed as $$[r_x, r_y, r_z, \dot{r}_x, \dot{r}_y, \dot{r}_z]^T$$

• delta_t (timedelta) – The time interval over which to estimate the universal anomaly

• grav_parameter (float, optional) – The universal gravitational parameter. Defaults to that of the Earth, $$3.986004418 \times 10^{14} \ \mathrm{m}^{3} \ \mathrm{s}^{-2}$$

• precision (float, optional) – For Newton’s method, the difference between new and old estimates of the universal anomaly below which the iteration stops and the answer is returned, (default = 1e-8)

• max_iterations (float, optional) – Maximum number of iterations allowed in while loop (default = 1e5)

Returns:

The universal anomaly, $$\chi$$

Return type:

numpy.ndarray

References

stonesoup.functions.orbital.lagrange_coefficients_from_universal_anomaly(o_state_vector, delta_t, grav_parameter=398600441800000.0, precision=1e-08, max_iterations=100000.0)[source]

Calculate the Lagrangian coefficients, f and g, and their time derivatives, by way of the universal anomaly and the Stumpff functions [2].

Parameters:
• o_state_vector (StateVector, StateVectors) – The (Cartesian) orbital state vector, $$[r_x, r_y, r_z, \dot{r}_x, \dot{r}_y, \dot{r}_z]^T$$

• delta_t (timedelta) – The time interval over which to calculate

• grav_parameter (float, optional) – The universal gravitational parameter. Defaults to that of the Earth, $$3.986004418 \times 10^{14} \ \mathrm{m}^{3} \ \mathrm{s}^{-2}$$. Note that the units of time must be seconds.

• precision (float, optional) – Precision to which to calculate the universal anomaly() (default = 1e-8). See the doc section for that function

• max_iterations (float, optional) – Maximum number of iterations in determining universal anomaly (default = 1e5)

Returns:

The Lagrange coefficients, $$f, g, \dot{f}, \dot{g}$$, in that order.

Return type:

References

stonesoup.functions.orbital.eccentric_anomaly_from_mean_anomaly(mean_anomaly, eccentricity, precision=1e-08, max_iterations=100000.0)[source]

Approximately solve the transcendental equation $$E - e sin E = M_e$$ for E. This is an iterative process using Newton’s method.

Parameters:
• mean_anomaly (float) – Current mean anomaly

• eccentricity (float) – Orbital eccentricity

• precision (float, optional) – Precision used for the stopping point in determining eccentric anomaly from mean anomaly, (default = 1e-8)

• max_iterations (float, optional) – Maximum number of iterations for the while loop, (default = 1e5)

Returns:

Eccentric anomaly of the orbit

Return type:

float

stonesoup.functions.orbital.tru_anom_from_mean_anom(mean_anomaly, eccentricity, precision=1e-08, max_iterations=100000.0)[source]

Get the true anomaly from the mean anomaly via the eccentric anomaly

Parameters:
• mean_anomaly (float) – The mean anomaly

• eccentricity (float) – Eccentricity

• precision (float, optional) – Precision used for the stopping point in determining eccentric anomaly from mean anomaly, (default = 1e-8)

• max_iterations (float, optional) – Maximum number of iterations in determining eccentric anomaly, (default = 1e5)

Returns:

True anomaly

Return type:

float

stonesoup.functions.orbital.perifocal_position(eccentricity, semimajor_axis, true_anomaly)[source]

The position vector in perifocal coordinates calculated from the Keplerian elements

Parameters:
• eccentricity (float) – Orbit eccentricity

• semimajor_axis (float) – Orbit semi-major axis

• true_anomaly – Orbit true anomaly

Returns:

$$[r_x, r_y, r_z]$$ position in perifocal coordinates

Return type:

numpy.array

stonesoup.functions.orbital.perifocal_velocity(eccentricity, semimajor_axis, true_anomaly, grav_parameter=398600441800000.0)[source]

The velocity vector in perifocal coordinates calculated from the Keplerian elements

Parameters:
• eccentricity (float) – Orbit eccentricity

• semimajor_axis (float) – Orbit semi-major axis

• true_anomaly (float) – Orbit true anomaly

• grav_parameter (float, optional) – Standard gravitational parameter $$\mu = G M$$. Default is $$3.986004418 \times 10^{14} \mathrm{m}^3 \mathrm{s}^{-2}$$

Returns:

$$[\dot{r}_x, \dot{r}_y, \dot{r}_z]$$ velocity in perifocal coordinates

Return type:

numpy.narray

stonesoup.functions.orbital.perifocal_to_geocentric_matrix(inclination, raan, argp)[source]

Return the matrix which transforms from perifocal to geocentric coordinates

Parameters:
• inclination (float) – Orbital inclination

• raan (float) – Orbit Right Ascension of the ascending node

• argp (float) – The orbit’s argument of periapsis

Returns:

The $$3 \times 3$$ array that transforms from perifocal coordinates to geocentric coordinates

Return type:

numpy.array

stonesoup.functions.orbital.keplerian_to_rv(state_vector, grav_parameter=398600441800000.0)[source]

Convert the Keplerian orbital elements to position, velocity state vector

Parameters:
• state_vector (StateVector, StateVectors) –

The Keplerian orbital state vector is defined as

$\begin{split}X = [e, a, i, \Omega, \omega, \theta]^{T} \\\end{split}$

where: $$e$$ is the orbital eccentricity (unitless), $$a$$ the semi-major axis (m), $$i$$ the inclination (rad), $$\Omega$$ is the longitude of the ascending node (rad), $$\omega$$ the argument of periapsis (rad), and $$\theta$$ the true anomaly (rad)

• grav_parameter (float, optional) – Standard gravitational parameter $$\mu = G M$$. The default is $$3.986004418 \times 10^{14} \mathrm{m}^3 \mathrm{s}^{-2}$$

Returns:

Orbital state vector as $$[r_x, r_y, r_z, \dot{r}_x, \dot{r}_y, \dot{r}_z]$$

Return type:

Warning

No checking undertaken. Assumes Keplerian elements rendered correctly as above

stonesoup.functions.orbital.mod_inclination(x)[source]

Calculates the modulus of an inclination. Inclination angles are within the range $$0$$ to $$\pi$$.

Parameters:

x (float) – inclination angle in radians

Returns:

Angle in radians in the range $$0$$ to $$+\pi$$

Return type:

float

stonesoup.functions.orbital.mod_elongitude(x)[source]

Calculates the modulus of an ecliptic longitude in which angles are within the range $$0$$ to $$2 \pi$$.

Parameters:

x (float) – longitudinal angle in radians

Returns:

Angle in radians in the range $$0$$ to $$+2 \pi$$

Return type:

float

## Interpolation

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

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: ) [source]

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.

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.