ADCS.estimators.attitude_determination module

Attitude determination from vector observations: TRIAD, QUEST and the q-method.

Given directions observed in the body frame (body) and the same directions known in the inertial frame (reference), find the attitude. These are the classic single-frame solutions of Wahba’s problem and are used to warm-start the recursive estimators from a first set of readings.

Background

Most attitude sensors measure a direction: the magnetometers give the direction of the magnetic field in the body frame, the sun sensors the direction to the sun, a horizon sensor the direction to the Earth, a star tracker the direction to a star. For each of these the same direction is also known in the inertial frame, from the field model, the sun ephemeris, the orbit, or the star catalogue. One such pair fixes two of the three attitude angles; the rotation about the measured direction itself is invisible to it. Two pairs that are not parallel fix the attitude completely, and more pairs over-determine it.

Wahba [1] posed the problem of using all pairs at once: find the rotation that maps the inertial directions onto the measured ones with the smallest weighted sum of squared residuals, each weight being the inverse variance of that observation’s angular error. Everything in this module solves, or approximates, that problem for one instant, without any dynamics or history, which is why these are called single-frame or “point” methods:

• TRIAD [2] uses exactly two pairs. It builds an orthonormal triad from the two directions in each frame and reads the rotation off the two triads. It is exact and needs no iteration, but it trusts the first direction completely and uses the second only to fix the rotation about the first, so it is not the optimal combination of the two. Its covariance is obtained by linearising the construction (triad_covariance()).

• The q-method [3] (Davenport) notes that Wahba’s loss is a quadratic form in the attitude quaternion, so the optimum is the eigenvector of a 4x4 symmetric matrix built from the observations, belonging to its largest eigenvalue. It takes any number of pairs with any weights and is exact up to the eigen-solver.

• QUEST [4] (Shuster and Oh, “QUaternion ESTimator”) reaches the same optimum without an eigen-solver, which is why most flight software uses it. The largest eigenvalue is close to the sum of the weights and is found by a Newton iteration on the characteristic polynomial; the quaternion then follows in closed form through the Gibbs vector, with the “sequential rotations” of the paper covering the 180-degree case where that vector is singular. The same paper gives the covariance of the optimal estimate, the small-angle attitude-error covariance [sum_i (1/sigma_i^2)(I - b_i b_i^T)]^-1 (wahba_covariance()), which is the best any estimator can do from those directions and is what makes the solution usable as the initial state and covariance of a filter.

QUEST and the q-method return the same attitude on exact data; here QUEST is additionally checked against the q-method in the tests, and both are polished by a few Gauss-Newton steps on the loss because the closed forms lose accuracy when one direction is weighted far more heavily than the others (see _refine()). Shepperd’s method [5] is used wherever a quaternion has to be read off a rotation matrix. Markley and Mortari [6] survey these and the later algorithms, and Markley and Crassidis [7] give a textbook treatment.

Why a single-frame solution is worth having

The recursive filters (EKF, MEKF, UKF) refine an attitude they are given; they cannot start from nothing. Started at an arbitrary attitude with a large covariance they spend their first part of a run converging, and because their corrections are linearised about the current estimate, a start that is far off can converge slowly, settle on a wrong solution, or diverge. A single-frame solution from the first readings is accurate to the sensors’ noise and comes with a consistent covariance, so the filter begins where its linearisation is valid. The same solution is useful on its own, as a sanity check of a filter (a filter that drifts away from the point solution is diverging) and as a plain attitude estimate when no dynamics model is wanted.

Conventions match the rest of the package: quaternions are Hamilton, scalar first, and rot_mat() maps body to inertial, so a unit reference direction r is observed as b = rot_mat(q).T @ r. Every function accepts one standard deviation per observation, in radians: the angular error of that direction, assumed the same in every direction perpendicular to it (the usual model behind QUEST).

References

[1]

G. Wahba, “A Least Squares Estimate of Satellite Attitude,” SIAM Review, Vol. 7, No. 3, 1965, p. 409. doi:10.1137/1007077

[2]

H. D. Black, “A Passive System for Determining the Attitude of a Satellite,” AIAA Journal, Vol. 2, No. 7, 1964, pp. 1350-1351. doi:10.2514/3.2555

[3]

P. B. Davenport, “A Vector Approach to the Algebra of Rotations with Applications,” NASA Technical Note TN D-4696, 1968. https://ntrs.nasa.gov/citations/19680021122

[4]

M. D. Shuster and S. D. Oh, “Three-Axis Attitude Determination from Vector Observations,” Journal of Guidance and Control, Vol. 4, No. 1, 1981, pp. 70-77. doi:10.2514/3.19717

[5]

S. W. Shepperd, “Quaternion from Rotation Matrix,” Journal of Guidance and Control, Vol. 1, No. 3, 1978, pp. 223-224. doi:10.2514/3.55767b

[6]

F. L. Markley and D. Mortari, “Quaternion Attitude Estimation Using Vector Observations,” Journal of the Astronautical Sciences, Vol. 48, No. 2-3, 2000, pp. 359-380.

[7]

F. L. Markley and J. L. Crassidis, Fundamentals of Spacecraft Attitude Determination and Control, Springer, 2014, Chapter 5. doi:10.1007/978-1-4939-0802-8

class ADCS.estimators.attitude_determination.AttitudeSolution(quaternion, covariance, loss, method)

Bases: object

The result of a single-frame attitude solution.

Parameters:

• quaternion (ndarray) – Attitude quaternion (scalar first, rot_mat convention).

• covariance (ndarray | None) – 3x3 covariance of the small-angle attitude error in the body frame, in rad^2 (rotation-vector units), or None when no measurement uncertainty was given.

• loss (float) – Wahba loss at the solution with weights normalised to sum to one; zero for perfect observations.

• method (str) – "triad", "quest" or "q_method".

covariance: ndarray | None

loss: float

method: str

quaternion: ndarray

ADCS.estimators.attitude_determination.q_method(body, reference, sigma, *, refine_steps=8)

Davenport’s q-method: the exact Wahba optimum from the 4x4 eigenproblem.

Slower than quest() in principle and free of special cases; QUEST is checked against it in the tests. Like QUEST it is polished by refine_steps Gauss-Newton steps, because the eigenvector of the 4x4 matrix is itself imprecise when one direction dominates the weights.

Parameters:

• body (ndarray)

• reference (ndarray)

• sigma (ndarray | float)

• refine_steps (int)

Return type:

AttitudeSolution

ADCS.estimators.attitude_determination.quaternion_from_rotation_matrix(matrix)

Return the scalar-first quaternion q with rot_mat(q) == matrix.

Shepperd’s method: the largest of the four squared components is computed first so no division by a small number occurs. The scalar part is made non-negative.

Parameters:

matrix (ndarray)

Return type:

ndarray

ADCS.estimators.attitude_determination.quest(body, reference, sigma, *, tolerance=1e-12, max_iterations=50, refine_steps=8)

QUEST (Shuster and Oh 1981): the Wahba optimum without an eigen-solver.

The largest eigenvalue of Davenport’s matrix is found by Newton iteration on its characteristic polynomial, starting from the sum of the weights, and the quaternion follows from the Gibbs vector. Near a 180 degree rotation the Gibbs vector is singular; the reference directions are then rotated by 180 degrees about a body axis (the “sequential rotations” of the original paper), the problem solved there, and the rotation undone.

refine_steps Gauss-Newton steps (at most; they stop once a step is below 1e-14 rad) polish the closed-form answer. They matter when one direction is weighted far more heavily than the others, where the closed form loses accuracy (see _refine()); 0 gives the textbook algorithm.

Parameters:

• body (ndarray)

• reference (ndarray)

• sigma (ndarray | float)

• tolerance (float)

• max_iterations (int)

• refine_steps (int)

Return type:

AttitudeSolution

ADCS.estimators.attitude_determination.triad(body_1, body_2, reference_1, reference_2, *, sigma_1=None, sigma_2=None)

TRIAD: the attitude from two directions, trusting the first one exactly.

The first direction is matched exactly and the second fixes the rotation about it, so put the more accurate direction first. Raises when the two directions are parallel (in either frame), where the rotation about them cannot be determined.

Parameters:

• body_1 (ndarray)

• body_2 (ndarray)

• reference_1 (ndarray)

• reference_2 (ndarray)

• sigma_1 (float | None)

• sigma_2 (float | None)

Return type:

AttitudeSolution

ADCS.estimators.attitude_determination.triad_covariance(body_1, body_2, reference_1, reference_2, sigma_1, sigma_2)

Covariance of the TRIAD solution, by linearising the construction.

TRIAD trusts the first direction completely, so its error is larger than the optimal (wahba_covariance()) one; the covariance is obtained by propagating each observation’s tangent-plane noise through the TRIAD map with central differences.

Parameters:

• body_1 (ndarray)

• body_2 (ndarray)

• reference_1 (ndarray)

• reference_2 (ndarray)

• sigma_1 (float)

• sigma_2 (float)

Return type:

ndarray

ADCS.estimators.attitude_determination.wahba_covariance(body, sigma)

Covariance of the optimal attitude estimate for the given observations.

P = [sum_i (1/sigma_i^2)(I - b_i b_i^T)]^-1 (Shuster and Oh 1981): the small-angle attitude-error covariance in the body frame, in rad^2. It is the best any estimator can do from these directions. With a single direction, or directions that are all parallel, the rotation about that direction is unobservable and a ValueError is raised.

Parameters:

• body (ndarray)

• sigma (ndarray | float)

Return type:

ndarray

ADCS.estimators.attitude_determination.wahba_loss(quaternion, body, reference, sigma)

Wahba loss 0.5 * sum w_i |b_i - rot_mat(q).T r_i|^2 with weights normalised to one.

Parameters:

• quaternion (ndarray)

• body (ndarray)

• reference (ndarray)

• sigma (ndarray | float)

Return type:

float