Skip to content

Commit

Permalink
Correct style in docstring.
Browse files Browse the repository at this point in the history
  • Loading branch information
Mayitzin committed Jan 23, 2021
1 parent d2e8c66 commit a9a6eda
Showing 1 changed file with 16 additions and 15 deletions.
31 changes: 16 additions & 15 deletions ahrs/filters/quest.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
QUaternion ESTimator as described by Shuster in [Shuster1981]_ and [Shuster1978]_.
We start to define the goal of finding an orthogonal matrix :math:`\\mathbf{A}`
that minimizes the loss function
that minimizes the loss function:
.. math::
L(\\mathbf{A}) = \\frac{1}{2}\\sum_{i=1}^n |\\hat{\\mathbf{W}}_i - \\mathbf{A}\\hat{\\mathbf{V}}_i|^2
Expand All @@ -15,10 +15,10 @@
:math:`\\hat{\\mathbf{V}}_i` are nonparallel **reference vectors**, and
:math:`\\hat{\\mathbf{W}}_i` are the corresponding **observation vectors**.
The gain function g(\\mathbf{A}) is defined by
The gain function :math:`g(\\mathbf{A})` is defined by
.. math::
g(\\mathbf{A}) = 1 - L(\\mathbf{A}) = \\sum_{i=1}^na_i\\hat{\\mathbf{W}}_i^T\\mathbf{A}\\hat{\\mathbf{V}}_i
g(\\mathbf{A}) = 1 - L(\\mathbf{A}) = \\sum_{i=1}^na_i\\,\\hat{\\mathbf{W}}_i^T\\mathbf{A}\\hat{\\mathbf{V}}_i
The loss function :math:`L(\\mathbf{A})` is at its minimum when the gain
function :math:`g(\\mathbf{A})` is at its maximum. The gain function can be
Expand All @@ -27,9 +27,8 @@
.. math::
g(\\mathbf{A}) = \\sum_{i=1}^na_i\\mathrm{tr}\\big(\\hat{\\mathbf{W}}_i^T\\mathbf{A}\\hat{\\mathbf{V}}_i\\big) = \\mathrm{tr}(\\mathbf{AB}^T)
where :math:`\\mathrm{tr}(\\mathbf{C})` is the `trace <https://en.wikipedia.org/wiki/Trace_(linear_algebra)>`_
of a matrix :math:`\\mathbf{C}`, and :math:`\\mathbf{B}` is the **attitude
profile matrix**:
where :math:`\\mathrm{tr}` is the `trace <https://en.wikipedia.org/wiki/Trace_(linear_algebra)>`_
of a matrix, and :math:`\\mathbf{B}` is the **attitude profile matrix**:
.. math::
\\mathbf{B} = \\sum_{i=1}^na_i\\hat{\\mathbf{W}}_i^T\\hat{\\mathbf{V}}_i
Expand All @@ -47,8 +46,9 @@
.. warning::
The definition of a quaternion used by Shuster sets the vector part
:math:`\\mathbf{Q}` followed by the scalar part :math:`q`. This module,
however will return the estimated quaternion with scalar part first and
followed by the vector part: :math:`\\bar{\\mathbf{q}} = \\begin{bmatrix}q & \\mathbf{Q}\\end{bmatrix}`
however, will return the estimated quaternion with the *scalar part first*
and followed by the vector part: :math:`\\bar{\\mathbf{q}} = \\begin{bmatrix}q
& \\mathbf{Q}\\end{bmatrix}`
Because the quaternion works as a versor, it must satisfy:
Expand Down Expand Up @@ -158,7 +158,8 @@
To find :math:`\\lambda` we implement the `Newton-Raphson method
<https://en.wikipedia.org/wiki/Newton%27s_method>`_ using the sum of the
weights :math:`a_i` as a starting value.
weights :math:`a_i` (in the beginning is constrained to be equal to 1) as a
starting value.
.. math::
\\lambda_{t+1} \\gets \\lambda_t - \\frac{f(\\lambda)}{f'(\\lambda)}
Expand All @@ -167,7 +168,7 @@
For sensor accuracies better than 1 arc-min (1 degree) the accuracy of a 64-bit
word is exhausted after only one iteration.
Finally, the optimal quaternion describing the attitude is found as:
Finally, the **optimal quaternion** describing the attitude is found as:
.. math::
\\bar{\\mathbf{q}}_\\mathrm{opt} = \\frac{1}{\\sqrt{\\gamma^2+|\\mathbf{Y}|^2}} = \\begin{bmatrix}\\mathbf{Y}\\\\ \\gamma \\end{bmatrix}
Expand All @@ -184,8 +185,8 @@
This solution can still lead to an indeterminant result if both :math:`\\gamma`
and :math:`\\mathbf{X}` vanish simultaneously. :math:`\\gamma` vanishes if and
only if the angle of rotation is equal :math:`\\pi`, even if :math:`\\mathbf{X}`
does not vanish along.
only if the angle of rotation is equal to :math:`\\pi`, even if
:math:`\\mathbf{X}` does not vanish along.
References
----------
Expand Down Expand Up @@ -218,11 +219,11 @@ class QUEST:
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.
Array with two weights. One per sensor measurement.
magnetic_dip : float
Magnetic Inclination angle, in degrees.
Local magnetic inclination angle, in degrees.
gravity : float
Normal gravity, in m/s^2.
Local normal gravity, in m/s^2.
Attributes
----------
Expand Down

0 comments on commit a9a6eda

Please sign in to comment.