M7-T3 — Attitude from Vector Observations: TRIAD and Wahba's Problem

Learning objectives

After this lesson, you should be able to:

• Compute the attitude matrix from two vector pairs with TRIAD and say which vector it trusts.
• State Wahba's problem and its singular-value solution.
• Explain why a two-vector solution fails as the vectors approach parallel.
• Choose weights for vectors of different accuracy.

The intuition

Linear Algebra M6-T5: Singular Value Decomposition supplies the matrix tool this lesson applies. The attitude question is as follows: a direction known in a reference frame (north, the Sun, a star) and measured in the body frame fixes the attitude except for a spin about that direction. This lesson turns that statement into algorithms.

One vector is not enough

If the body measures the Sun direction, the attitude is known to within a rotation about the Sun line: any such rotation leaves the measurement unchanged. A second direction, not parallel to the first, resolves it. The two measured and two reference directions together determine the attitude completely, with some redundancy because the angle between two vectors is the same in both frames (and any difference is a measurement error).

TRIAD: build a frame from each pair

TRIAD builds an orthonormal triad from the two vectors in each frame: the first axis along the first vector, the second along their cross product, the third completing the right-handed set. The attitude matrix is what rotates one triad into the other. It treats the first vector as exact and uses the second only for the rotation about the first, so the order of the vectors is a choice: the Sun sensor (0.1°) should go first, the magnetometer (1°) second. Swapping them gives 1.41° instead of 0.99° in the worked example.

Wahba: the best attitude for any number of vectors

Wahba's problem asks for the rotation that best maps the reference vectors onto the measured ones, in the weighted least-squares sense. The solution is in closed form: form a 3×3 matrix from the weighted outer products, take its singular value decomposition, and the attitude is the product of the two orthogonal factors, with a sign fix for the determinant. It treats every vector according to its weight, and with unequal accuracies and weights proportional to the inverse variance it comes very close to TRIAD with the best vector first.

Geometry limits everything

As the two references approach parallel, the second vector adds less and less information about the spin about the first. With a 1° error on the second and a separation of 5°, the rotation about the first vector is uncertain by about 1°/sin 5° = 11.5°. All four algorithms follow this law within a few percent: it comes from the geometry of the measurement, not from the method.

Hopper has no Sun sensor to speak of, but it has the same structure with gravity and the magnetic field: levelling and gyrocompassing or a magnetometer are two direction pairs, and the example below uses a satellite only because its two sensors differ so much in accuracy. At mid latitudes the field dips roughly 60° below the horizontal, about 30° from the vertical, so by the law below the heading is about twice as uncertain as the field measurement; near the poles the field is nearly parallel to gravity and the heading is lost.

Attitude from vectors is limited by the angle between them and by their accuracy. TRIAD with the better vector first is nearly as good as the optimal solution when one vector is much better than the other, and no algorithm repairs a bad geometry.

Definitions

Direction pair

A unit vector known in the reference frame, $\mathbf{r}_i$, and its measurement in the body frame, $\mathbf{b}_i \approx \mathbf{A}\mathbf{r}_i$, with $\mathbf{A}$ the attitude matrix (reference to body).

TRIAD

The two-vector algorithm that builds an orthonormal frame from each pair and returns $\mathbf{A} = \mathbf{T}_b\mathbf{T}_r^{\mathsf{T}}$.

Wahba's problem

Minimize $L(\mathbf{A}) = \tfrac12\sum_i w_i\|\mathbf{b}_i - \mathbf{A}\mathbf{r}_i\|^2$ over rotation matrices.

Attitude profile matrix $\mathbf{B}$

$\mathbf{B} = \sum_i w_i\,\mathbf{b}_i\mathbf{r}_i^{\mathsf{T}}$.

Derivation: Wahba's solution, and the 1/sin law

Minimizing the loss over rotations reduces to a trace maximization.

1. $$L(\mathbf{A}) = \tfrac12\sum_i w_i\|\mathbf{b}_i - \mathbf{A}\mathbf{r}_i\|^2 = \sum_i w_i - \sum_i w_i\,\mathbf{b}_i^{\mathsf{T}}\mathbf{A}\mathbf{r}_i$$

   Expand the squared norm; with unit vectors and an orthogonal A the quadratic terms are $\|\mathbf{b}\|^2 + \|\mathbf{A}\mathbf{r}\|^2 = 2$, constants.

2. $$\sum_i w_i\,\mathbf{b}_i^{\mathsf{T}}\mathbf{A}\mathbf{r}_i = \mathrm{tr}\bigl(\mathbf{A}\,\mathbf{B}^{\mathsf{T}}\bigr), \quad \mathbf{B} = \sum_i w_i\mathbf{b}_i\mathbf{r}_i^{\mathsf{T}}$$

   Write the sum as a trace using the cyclic property; this introduces the attitude profile matrix.

3. $$\max_{\mathbf{A}}\ \mathrm{tr}(\mathbf{A}\mathbf{V}\boldsymbol{\Sigma}\mathbf{U}^{\mathsf{T}}) = \max\ \mathrm{tr}(\boldsymbol{\Sigma}\,\mathbf{U}^{\mathsf{T}}\mathbf{A}\mathbf{V})$$

   Insert the SVD and use the cyclic property of the trace again.

4. $$\mathbf{U}^{\mathsf{T}}\mathbf{A}\mathbf{V} = \mathrm{diag}(1,\ 1,\ \det\mathbf{U}\det\mathbf{V})$$

   The matrix $\mathbf{U}^{\mathsf{T}}\mathbf{A}\mathbf{V}$ is orthogonal, so its diagonal entries are at most 1 in magnitude, and the trace against the nonnegative singular values is largest when it is the identity, up to the sign needed for a proper rotation. Solve for A.

Why the error goes as 1/sin

1. $$\mathbf{r}_2 = \cos\alpha\,\mathbf{r}_1 + \sin\alpha\,\mathbf{e}$$

   Resolve the second reference into a part along the first and a part perpendicular, at the separation angle $\alpha$.

2. $$\delta\phi_1 \approx \sigma_1,\ \ \delta\phi_{\parallel} \approx \dfrac{\sigma_2}{\sin\alpha}$$

   A rotation about the first vector by a small angle $\phi$ moves the second vector by $\phi\sin\alpha$, so an error of $\sigma_2$ in the second measurement is an error of $\sigma_2/\sin\alpha$ in the rotation about the first.

3. $$\sigma_{\mathrm{att}} \approx \sigma_2/\sin\alpha: \ 1^\circ/\sin 5^\circ = 11.5^\circ$$

   For the satellite example: the field vector at 5° separation gives about 11.5°, as the simulation shows (11.9°).

Wahba's optimal attitude is the orthogonal factor of the attitude profile matrix. All two-vector solutions share the 1/sin law: the second vector tells you the rotation about the first only as well as its component perpendicular to the first.

A worked example

A satellite measures the Sun direction (0.1° per component) and the magnetic field (1.0°); the true attitude is a 40° rotation about the axis (1, 2, 3). The angle between the reference vectors is varied; each point is the rms of 3000 trials.

1. Noise-free check. With exact vectors, TRIAD and the Wahba solution both return the true attitude (error 0.0°, to rounding).
2. Good geometry (90°). TRIAD with the Sun first gives 0.993°, with the field first 1.413°, Wahba with weights $1/\sigma^2$ gives 0.992° and with equal weights 1.110°.
3. Shrinking separation. At 45°, 20°, 10° and 5° the TRIAD (Sun first) error is 1.43°, 2.94°, 5.78° and 11.86°: it follows 1°/sin of the angle, up to a constant, within a few per cent.
4. Algorithm differences. The weighted Wahba solution is within 0.001° of TRIAD with the Sun first, because the Sun vector is 100 times more accurate in variance terms. Equal weights are better than TRIAD with the wrong vector first, and worse than the right order.
5. Implementation note. Two vectors give a rank-two profile matrix, so the library computes the orthogonal factor by adding a vanishing third pair, the cross products, with weight 10⁻⁹ times the smallest, and then taking the polar factor by Newton iteration, which equals the singular-value solution.

import math
import random
import gnckit as g

D2R = math.pi / 180
def norm(v): n = math.sqrt(sum(c * c for c in v)); return [c / n for c in v]
def cross(a, b): return [a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]]
def dot(a, b): return sum(x * y for x, y in zip(a, b))
def mv(M, v): return [sum(M[i][j] * v[j] for j in range(3)) for i in range(3)]
def outer(a, b): return [[a[i] * b[j] for j in range(3)] for i in range(3)]

def triad_frame(v1, v2):
    t1 = norm(v1); t2 = norm(cross(v1, v2)); t3 = cross(t1, t2)
    return [[t1[i], t2[i], t3[i]] for i in range(3)]          # columns t1 t2 t3

def triad(b1, b2, r1, r2):
    """A with b = A r: A = [t1 t2 t3]_b [t1 t2 t3]_r^T, v1 is trusted completely."""
    return g.mat_mul(triad_frame(b1, b2), g.mat_T(triad_frame(r1, r2)))

def polar(M, iters=80):
    X = [row[:] for row in M]
    for _ in range(iters):
        Xi = g.mat_T(g.mat_inv(X))
        X = [[0.5 * (X[i][j] + Xi[i][j]) for j in range(3)] for i in range(3)]
    return X

def wahba(bs, rs, ws):
    if len(bs) == 2:                      # B has rank 2: the third axis is fixed by det A = +1; a vanishing third pair makes the polar factor well defined
        bs = bs + [norm(cross(bs[0], bs[1]))]; rs = rs + [norm(cross(rs[0], rs[1]))]; ws = ws + [1e-9 * min(ws)]
    B = [[0.0] * 3 for _ in range(3)]
    for b, r, w in zip(bs, rs, ws):
        o = outer(b, r)
        B = [[B[i][j] + w * o[i][j] for j in range(3)] for i in range(3)]
    A = polar(B)                                           # = U diag(1, 1, det U det V) V^T for det B > 0
    return A, B

def rot(axis, ang):
    ax = norm(axis); c, s = math.cos(ang), math.sin(ang)
    K = [[0, -ax[2], ax[1]], [ax[2], 0, -ax[0]], [-ax[1], ax[0], 0]]
    return [[(1 if i == j else 0) * c + (1 - c) * ax[i] * ax[j] + s * K[i][j] for j in range(3)] for i in range(3)]

def angle_of(A):
    return math.acos(max(-1.0, min(1.0, (A[0][0] + A[1][1] + A[2][2] - 1) / 2)))

Atrue = rot([1, 2, 3], 40 * D2R)
random.seed(5)
def perturb(v, sd):
    d = [random.gauss(0, sd) for _ in range(3)]
    return norm([v[i] + d[i] for i in range(3)])

print('A satellite measures the Sun direction (sd 0.1 deg per component) and the magnetic field (sd 1.0 deg).')
r_sun = norm([1, 0, 0])
def trial(sep_deg, n=3000):
    # reference field at the given angle from the Sun direction
    s = sep_deg * D2R
    r_mag = norm([math.cos(s), math.sin(s), 0])
    e_tr, e_wa, e_tr2, e_eq = [], [], [], []
    for _ in range(n):
        b_sun = perturb(mv(Atrue, r_sun), 0.1 * D2R)
        b_mag = perturb(mv(Atrue, r_mag), 1.0 * D2R)
        A1 = triad(b_sun, b_mag, r_sun, r_mag)
        A2 = triad(b_mag, b_sun, r_mag, r_sun)
        A3, _B = wahba([b_sun, b_mag], [r_sun, r_mag], [1 / (0.1 * D2R) ** 2, 1 / (1.0 * D2R) ** 2])
        A4, _B = wahba([b_sun, b_mag], [r_sun, r_mag], [1.0, 1.0])
        d = lambda A: angle_of(g.mat_mul(A, g.mat_T(Atrue)))
        e_tr.append(d(A1)); e_tr2.append(d(A2)); e_wa.append(d(A3)); e_eq.append(d(A4))
    rms = lambda e: math.sqrt(sum(x * x for x in e) / len(e)) / D2R
    return rms(e_tr), rms(e_tr2), rms(e_wa), rms(e_eq)
print('  angle between the two reference vectors | TRIAD (Sun first) | TRIAD (field first) | Wahba, weighted | Wahba, equal weights (rms attitude error, deg)')
for sep in (90, 45, 20, 10, 5):
    a, b_, c, e4 = trial(sep)
    print(f'  {sep:3d} deg                                  {a:12.3f}     {b_:14.3f}        {c:10.3f}      {e4:10.3f}')
# exact data: both exact
b1, b2 = mv(Atrue, r_sun), mv(Atrue, norm([math.cos(0.8), math.sin(0.8), 0]))
print(f'noise-free check: TRIAD error {angle_of(g.mat_mul(triad(b1, b2, r_sun, norm([math.cos(0.8), math.sin(0.8), 0])), g.mat_T(Atrue))) / D2R:.1e} deg, Wahba error {angle_of(g.mat_mul(wahba([b1, b2], [r_sun, norm([math.cos(0.8), math.sin(0.8), 0])], [1, 1])[0], g.mat_T(Atrue))) / D2R:.1e} deg')

A satellite measures the Sun direction (sd 0.1 deg per component) and the magnetic field (sd 1.0 deg).
  angle between the two reference vectors | TRIAD (Sun first) | TRIAD (field first) | Wahba, weighted | Wahba, equal weights (rms attitude error, deg)
   90 deg                                         0.993              1.413             0.992           1.110
   45 deg                                         1.433              1.746             1.433           1.516
   20 deg                                         2.941              3.100             2.941           2.981
   10 deg                                         5.776              5.858             5.776           5.796
    5 deg                                        11.856             11.898            11.856          11.867
noise-free check: TRIAD error 0.0e+00 deg, Wahba error 0.0e+00 deg

In robotics

Vector observations are how almost every spacecraft and most small vehicles get an attitude without a gyro.

• Spacecraft. Sun sensors, star trackers, horizon sensors and magnetometers each give directions, and the attitude determination of a satellite is a Wahba problem weighted by their accuracies. A star tracker with several stars in view is a many-vector problem solved by the same equations.
• Small vehicles. Gravity (from the accelerometers at rest) and the magnetic field give roll, pitch and heading, as in levelling and magnetic compassing. A drone's attitude reference is a Wahba problem with two vectors of very different accuracy.
• Geometry in practice. A magnetometer near the pole is nearly parallel to gravity (the field points steeply down), so the heading is poor: the 1/sin law in action.
• As a starting value. A filter is initialized with the Wahba solution, and in-flight a gyro carries the attitude between vector updates, which is the next lesson.
• Beyond Wahba. Faster algorithms (QUEST, the Davenport q-method) give the same answer without an SVD, and covariance-aware variants give the uncertainty of the result. The answer is the same optimum.

The lab M7-L2: Two Vectors, One Attitude implements TRIAD and Wahba for this satellite and compares them as the vectors approach parallel. The attitude from vectors feeds the filters of M7-T4: Attitude Filters: Complementary and Multiplicative.

Common misconceptions

Key takeaways

1. Each vector pair fixes two attitude angles; two non-parallel pairs fix all three.
2. TRIAD builds a frame from each pair and returns $\mathbf{A} = \mathbf{T}_b\mathbf{T}_r^{\mathsf{T}}$, trusting the first vector exactly.
3. Wahba's solution is $\mathbf{U}\,\mathrm{diag}(1,1,\det\mathbf{U}\det\mathbf{V})\,\mathbf{V}^{\mathsf{T}}$ for $\mathbf{B} = \mathbf{U}\boldsymbol{\Sigma}\mathbf{V}^{\mathsf{T}}$; weights are the inverse variances.
4. The error of a two-vector solution is about $\sigma/\sin\alpha$: 0.99° at 90° and 11.9° at 5° separation for a 1° second vector.
5. TRIAD with the better vector first matched the weighted Wahba solution to 0.001°.
6. No algorithm can repair a bad geometry; only another direction can.

References and further reading

Each source is marked free or paid. Free to read does not mean free to reuse: the course resources page lists every source with its licence.

• G. Wahba, "A least squares estimate of satellite attitude", SIAM Review 7(3), 1965, the problem statement. Paid; a reprint is on the SIAM site.
• F. L. Markley and D. Mortari, "Quaternion attitude estimation using vector observations", Journal of the Astronautical Sciences 48(2-3), 2000, a review of TRIAD, the q-method and QUEST. Paid.
• J. R. Wertz (ed.), Spacecraft Attitude Determination and Control, Springer, 1978, the standard reference for sensors and methods. Paid.
• MIT 16.851 Satellite Engineering (OpenCourseWare) , for attitude determination in the context of satellite design. Free; licence CC BY-NC-SA 4.0.

Knowledge check

Three questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

Related content

Nothing below is required to finish this lesson.

• Before
  M7-T2: Coupling GNSS and INS: Loose, Tight and Deep

  Write the error state and the measurement matrix of a loosely and a tightly coupled filter, and explain what each still delivers with fewer than four satellites.

• Lab
  M7-L2: Two Vectors, One Attitude

  A satellite measures the Sun and the magnetic field with different accuracies. Implement TRIAD and the Wahba solution, and compare their errors as the two vectors approach parallel.

Other courses on this site treat neighbouring topics from their own angle:

• UAV M2-T1: GNSS fundamentals
• UAV M2-T3: GNSS receivers and positioning
• UAV M2-T4: GNSS errors and multipath
• UAV M2-T6: GNSS jamming and spoofing
• UAV M3-T6: Error-State Kalman Filter
• UAV M3-T5: Extended Kalman Filter
• UAV M3-T11: Multi-sensor fusion architectures
• UAV M3-T13: Innovation monitoring
• UAV M3-T16: Sensor fault detection and isolation
• UAV M6-T5: PX4 state estimation and EKF2
• UAV M2-T11: Visual-Inertial Odometry
• SLAM M8-T5: GPS-Denied Localization
• Linear Algebra M5-T6: Weighted Least Squares
• Linear Algebra M6-T5: Singular Value Decomposition
• Probability for Robotics (modules 7, 8 and 12: Kalman, extended and error-state filters)

This course leaves the following to the pages that already teach them:

• Constellations, signals, error sources, multipath, jamming and spoofing
• The Kalman, extended and error-state filter derivations
• Visual, LiDAR and radar odometry, VIO and SLAM as aiding sources
• Innovation tests (NIS, NEES) as such