Function navtk::solve_wahba_davenport(const Matrix3&, const Vector3&)

Function Documentation

std::vector<Matrix3> navtk::solve_wahba_davenport(const Matrix3 &outer, const Vector3 &cr)

Implements the “Davenports’ Q” method of determining the rotation between two fixed frames from common measurements in each of the frames, as described in ‘Survey of Nonlinear Attitude Estimation Methods’, Crassidis, Markley and Cheng.

http://www.acsu.buffalo.edu/~johnc/att_survey07.pdf.

Parameters

  • outer – Given N platform \(p\) and reference frame measurements \(r\), the sum of the possibly weighted \(a_i\) outer products of the platform and reference frame observations \( \sum\limits^N_{i=1}a_ip_ir_i^T \).

  • cr – Given the same set of observations used in outer, the sum of the cross products \( \sum\limits^N_{i=1}a_ip_i \times r_i \).

Returns

A set of reference to platform DCMs \( C_{ref}^{platform}\) that minimize the loss function \( J(C) = \frac{1}{2}\sum\limits^m_{i = 1}a_i|pb_i - C_{ref}^{platform}r_i||^2 \). If the output is of size greater than 1, then a unique solution could not be determined from the inputs. The solutions will be ordered from most to least likely, based upon the associated eigenvalue magnitudes.