# 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.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_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]

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

## 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) – input parameter, $$z$$

Returns:

Output value, $$S(z)$$

Return type:

float

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) – input parameter, $$z$$

Returns:

Output value, $$C(z)$$

Return type:

float

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) – 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:

float

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) – 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) –

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:

StateVector

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