# 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

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