Davenport’s q-Method#

In 1965 Grace Wahba came up with a simple, yet very intuitive, way to describe the problem of finding a rotation between two coordinate systems.

Given a set of \(N\) vector measurements \(\mathbf{u}\) in the body coordinate system, an optimal attitude matrix \(\mathbf{A}\) would minimize the loss function:

\[L(\mathbf{A}) = \frac{1}{2}\sum_{i=1}^Nw_i|u_i-\mathbf{A}v_i|^2\]

where \(u_i\) is the i-th vector measurement in the body frame, \(v_i\) is the i-th vector in the reference frame, and \(w_i\) are a set of \(N\) nonnegative weights for each observation. This famous formulation is known as Wahba’s problem.

A first elegant solution was proposed by [Davenport1968] that solves this in terms of quaternions, yielding a unique optimal solution. The corresponding gain function is defined as:

\[g(\mathbf{A}) = 1 - L(\mathbf{A}) = \sum_{i=1}^Nw_i\mathbf{U}^T\mathbf{AV}\]

The gain function is at maximum when the loss function \(L(\mathbf{A})\) is at minimum. The goal is, then, to find the optimal attitude matrix \(\mathbf{A}\), which maximizes \(g(\mathbf{A})\). We first notice that:

\[\begin{split}\begin{array}{rl} g(\mathbf{A}) =& \sum_{i=1}^Nw_i\mathrm{tr}\big(\mathbf{U}_i^T\mathbf{AV}_i\big) \\ =& \mathrm{tr}(\mathbf{AB}^T) \end{array}\end{split}\]

where \(\mathrm{tr}\) denotes the trace of a matrix, and \(\mathbf{B}\) is the attitude profile matrix:

\[\mathbf{B} = \sum_{i=1}^Nw_i\mathbf{UV}\]

Now, we must parametrize the attitude matrix in terms of a quaternion \(\mathbf{q}\):

\[\mathbf{A}(\mathbf{q}) = (q_w^2-\mathbf{q}_v\cdot\mathbf{q}_v)\mathbf{I}_3+2\mathbf{q}_v\mathbf{q}_v^T-2q_w\lfloor\mathbf{q}\rfloor_\times\]

where \(\mathbf{I}_3\) is a \(3\times 3\) identity matrix, and the expression \(\lfloor \mathbf{x}\rfloor_\times\) is the skew-symmetric matrix of a vector \(\mathbf{x}\). See the quaternion page for further details about this representation mapping.

The gain function, in terms of quaternion, becomes:

\[g(\mathbf{q}) = (q_w^2-\mathbf{q}_v\cdot\mathbf{q}_v)\mathrm{tr}\mathbf{B}^T + 2\mathrm{tr}\big(\mathbf{q}_v\mathbf{q}_v^T\mathbf{B}^T\big) + 2q_w\mathrm{tr}(\lfloor\mathbf{q}\rfloor_\times\mathbf{B}^T)\]

A simpler expression, using helper quantities, can be a bilinear relationship of the form:

\[g(\mathbf{q}) = \mathbf{q}^T\mathbf{Kq}\]

where the \(4\times 4\) matrix \(\mathbf{K}\) is built with:

\[\begin{split}\mathbf{K} = \begin{bmatrix} \sigma & \mathbf{z}^T \\ \mathbf{z} & \mathbf{S}-\sigma\mathbf{I}_3 \end{bmatrix}\end{split}\]

using the intermediate values:

\[\begin{split}\begin{array}{rcl} \sigma &=& \mathrm{tr}\mathbf{B} \\ \mathbf{S} &=& \mathbf{B}+\mathbf{B}^T \\ \mathbf{z} &=& \begin{bmatrix}B_{23}-B_{32} \\ B_{31}-B_{13} \\ B_{12}-B_{21}\end{bmatrix} \end{array}\end{split}\]

The optimal quaternion \(\hat{\mathbf{q}}\), which parametrizes the optimal attitude matrix, is an eigenvector of \(\mathbf{K}\). With the help of Lagrange multipliers, \(g(\mathbf{q})\) is maximized if the eigenvector corresponding to the largest eigenvalue \(\lambda\) is chosen.

\[\mathbf{K}\hat{\mathbf{q}} = \lambda\hat{\mathbf{q}}\]

The biggest disadvantage of this method is its computational load in the last step of computing the eigenvalues and eigenvectors to find the optimal quaternion.

References

[Davenport1968]

Paul B. Davenport. A Vector Approach to the Algebra of Rotations with Applications. NASA Technical Note D-4696. August 1968. (https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19680021122.pdf)

[Lerner2]

Lerner, G. M. “Three-Axis Attitude Determination” in Spacecraft Attitude Determination and Control, edited by J.R. Wertz. 1978. p. 426-428.

class ahrs.filters.davenport.Davenport(acc: ndarray | None = None, mag: ndarray | None = None, **kw)#

Davenport’s q-Method for attitude estimation

Parameters:

  • acc (numpy.ndarray, default: None) – N-by-3 array with measurements of acceleration in in m/s^2

  • mag (numpy.ndarray, default: None) – N-by-3 array with measurements of magnetic field in mT

  • weights (array-like) – Array with two weights used in each observation.

  • magnetic_dip (float) – Magnetic Inclination angle, in degrees. Defaults to magnetic dip of Munich, Germany.

  • gravity (float) – Normal gravity, in m/s^2. Defaults to normal gravity in Munich, Germany.

Variables:

  • acc (numpy.ndarray) – N-by-3 array with N accelerometer samples.

  • mag (numpy.ndarray) – N-by-3 array with N magnetometer samples.

  • w (numpy.ndarray) – Weights of each observation.

Raises:

ValueError – When dimension of input arrays acc and mag are not equal.

estimate(acc: ndarray | None = None, mag: ndarray | None = None) → ndarray#

Attitude Estimation

Parameters:

  • acc (numpy.ndarray) – Sample of tri-axial Accelerometer in m/s^2

  • mag (numpy.ndarray) – Sample of tri-axial Magnetometer in T

Returns:

q – Estimated attitude as a quaternion.

Return type:

numpy.ndarray