Function navtk::solve_wahba_davenport(const std::vector<Vector3>&, const std::vector<Vector3>&)

Function Documentation

std::vector<Matrix3> navtk::solve_wahba_davenport(const std::vector<Vector3> &p, const std::vector<Vector3> &r)

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

  • p – A collection of platform frame observations.

  • r – A collection of reference frame observations, the same size as p, where the ith observation in both p and r are of the same event. If p and r are different sizes, or either contains fewer than 2 measurements, a warning will be generated or an error thrown, depending on the value of navtk::ErrorMode.

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}||p_i - C_{ref}^{platform}r_i||^2 \), which differs slightly from the reference document in that all observations are weighted equally. 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. In the case of a warning being generated due to input sizes, a single navtk::zeros(3, 3) will be returned.