Correct equation of Taylor series in Quaternion Linearization.

ahrs/filters/angular.py

@@ -166,7 +166,8 @@
 
 Using the `Euler-Rodrigues rotation formula
 <https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Using_quaternion_as_rotations>`_
-and the exponential map from above we find a **closed-form solution** [Wertz]_:
+and the exponential map from above we find a **closed-form solution**
+:cite:p:`spence1978`:
 
 .. math::
     \\mathbf{q}_{t+1} =
@@ -199,7 +200,7 @@
     \\frac{1}{3!}\\dddot{\\mathbf{q}}_t\\Delta t^3 + \\cdots
 
 Using the definition of :math:`\\dot{\\mathbf{q}}` the new orientation
-:math:`\\mathbf{q}_{t+1}` is written as [Wertz]_:
+:math:`\\mathbf{q}_{t+1}` is written as :cite:p:`spence1978`:
 
 .. math::
     \\begin{array}{rcl}
@@ -208,8 +209,8 @@
     \\frac{1}{3!}\\Big(\\frac{1}{2}\\boldsymbol\\Omega(\\boldsymbol\\omega)\\Delta t\\Big)^3 + \\cdots\\Bigg]\\mathbf{q}_t \\\\
     && \\qquad{} + \\frac{1}{4}\\dot{\\boldsymbol\\Omega}(\\boldsymbol\\omega)\\Delta t^2\\mathbf{q}_t
     + \\Big[\\frac{1}{12}\\dot{\\boldsymbol\\Omega}(\\boldsymbol\\omega)\\boldsymbol\\Omega(\\boldsymbol\\omega)
-    + \\frac{1}{24}\\boldsymbol\\Omega(\\boldsymbol\\omega)\\dot{\\boldsymbol\\Omega}(\\boldsymbol\\omega)
-    + \\frac{1}{12}\\ddot{\\boldsymbol\\Omega}(\\boldsymbol\\omega)\\Big]\\Delta t^3\\mathbf{q}_t
+    + \\frac{1}{24}\\boldsymbol\\Omega(\\boldsymbol\\omega)\\dot{\\boldsymbol\\Omega}(\\boldsymbol\\omega)\\Big]\\Delta t^3\\mathbf{q}_t
+    + \\frac{1}{12}\\ddot{\\boldsymbol\\Omega}(\\boldsymbol\\omega)\\Delta t^3\\mathbf{q}_t
     + \\cdots
     \\end{array}
 
@@ -222,18 +223,18 @@
     \\frac{1}{2!}\\Big(\\frac{1}{2}\\boldsymbol\\Omega(\\boldsymbol\\omega)\\Delta t\\Big)^2 +
     \\frac{1}{3!}\\Big(\\frac{1}{2}\\boldsymbol\\Omega(\\boldsymbol\\omega)\\Delta t\\Big)^3 + \\cdots\\Bigg]\\mathbf{q}_t
 
-The error of the approximation vanishes rapidly at higher orders, or when the
-time step :math:`\\Delta t \\to 0`. The more terms we have, the better our
-approximation should be, assuming the sensor signals are unbiased and noiseless,
-with the downside of a big computational demand.
-
 Notice the series for :math:`\\mathbf{q}_{t+1}` also follows the form of the
 matrix exponential:
 
 .. math::
     e^{\\frac{\\Delta t}{2}\\boldsymbol\\Omega(\\boldsymbol\\omega)} =
     \\sum_{k=0}^\\infty \\frac{1}{k!} \\Big(\\frac{\\Delta t}{2}\\boldsymbol\\Omega(\\boldsymbol\\omega)\\Big)^k
 
+The error of the approximation vanishes rapidly at higher orders (:math:`k \\to
+0`), or when the time step :math:`\\Delta t \\to 0`. The more terms we have,
+the better our approximation should be, assuming the sensor signals are
+unbiased and noiseless, with the downside of a big computational demand.
+
 For our purpose a truncation up to the second term, making it of first order
 (:math:`k=1`), is implemented.