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
- 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
- stonesoup.functions.jacobian(fun, x)[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 aStateVector
of shape (Ns, 1) representing the Gaussian mean and aCovarianceMatrix
of shape (Ns, Ns) which is the covariance of the distributionalpha (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 thestate_vector
attribute in these States will be meaningful. Other quantities, likecovar
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 weightsnumpy.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
sigma_points (
StateVectors
of shape (Ns, 2*Ns+1)) – An array containing the locations of the sigma pointsmean_weights (
numpy.ndarray
of shape (2*Ns+1,)) – An array containing the sigma point mean weightscovar_weights (
numpy.ndarray
of shape (2*Ns+1,)) – An array containing the sigma point covariance weightscovar_noise (
CovarianceMatrix
of shape (Ns, Ns), optional) – Additive noise covariance matrix (default is None)
- Returns
StateVector
of shape (Ns, 1) – Calculated meanCovarianceMatrix
of shape (Ns, Ns) – Calculated covariance
- 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
sigma_points (
StateVectors
of shape (Ns, 2*Ns+1)) – An array containing the locations of the sigma pointsmean_weights (
numpy.ndarray
of shape (2*Ns+1,)) – An array containing the sigma point mean weightscovar_weights (
numpy.ndarray
of shape (2*Ns+1,)) – An array containing the sigma point covariance weightsfun (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 meanCovarianceMatrix
of shape (Ns, Ns) – Transformed covarianceCovarianceMatrix
of shape (Ns,Nm) – Calculated cross-covariance matrixStateVectors
of shape (Ns, 2*Ns+1) – An array containing the locations of the transformed sigma pointsnumpy.ndarray
of shape (2*Ns+1,) – An array containing the transformed sigma point mean weightsnumpy.ndarray
of shape (2*Ns+1,) – An array containing the transformed sigma point covariance weights
- 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 componentscovars (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 GaussianCovarianceMatrix
– The covariance of the reduced/single Gaussian
- stonesoup.functions.mod_bearing(x)[source]¶
Calculates the modulus of a bearing. Bearing angles are within the range \(-\pi\) to \(\pi\).
- stonesoup.functions.mod_elevation(x)[source]¶
Calculates the modulus of an elevation angle. Elevation angles are within the range \(-\pi/2\) to \(\pi/2\).
- stonesoup.functions.build_rotation_matrix(angle_vector: numpy.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) –the (about) –
aircraft/radar terms these correspond to (In) –
[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.
The result for vectors of length \(n\) is \(\Sigma_i^n a_i b_i\).
Inputs are state vectors, i.e. the second dimension is 1
- Parameters
a (StateVector) – A state vector
b (StateVector) – A state vector of equal length to \(a\)
- Returns
A scalar value representing the dot product of the vectors.
- Return type
- 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
- Returns
Final value for the time in last value in
t_values
- Return type
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}\]
- 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}\]
- 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
References
- 1
Curtis H.D. 2010, Orbital Mechanics for Engineering Students, 3rd Ed., Elsevier
- 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 functionmax_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
- 2
Bond V.R., Allman M.C. 1996, Modern Astrodynamics: Fundamentals and Perturbation Methods, Princeton University Press
- 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
- 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
- stonesoup.functions.orbital.perifocal_position(eccentricity, semimajor_axis, true_anomaly)[source]¶
The position vector in perifocal coordinates calculated from the Keplerian elements
- 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
- 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
- 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
Warning
No checking undertaken. Assumes Keplerian elements rendered correctly as above