In the Attitude From Tilt, the documentation assigns roll=θ, and pitch=φ. However, in calculation of ψ the roll and pitch symbols have been swapped. This makes sense, since the original NXP documentation assigns the symbols this way - and was likely an error when translating the equation.
The code is still correct, however the documentation should be updated to match.
|
.. math:: |
|
\\begin{array}{cl} |
|
\\mathbf{b} &= |
|
R_y(-\\theta)R_x(-\\phi)\\mathbf{m} = R_y(\\theta)^TR_x(\\phi)^T\\mathbf{m} \\\\ |
|
&= |
|
\\begin{bmatrix} |
|
\\cos\\theta & \\sin\\theta\\sin\\phi & \\sin\\theta\\cos\\phi \\\\ |
|
0 & \\cos\\phi & -\\sin\\phi \\\\ |
|
-\\sin\\theta & \\cos\\theta\\sin\\phi & \\cos\\theta\\cos\\phi |
|
\\end{bmatrix} |
|
\\begin{bmatrix}m_x \\\\ m_y \\\\ m_z\\end{bmatrix} \\\\ |
|
\\begin{bmatrix}b_x \\\\ b_y \\\\ b_z\\end{bmatrix} &= |
|
\\begin{bmatrix} |
|
m_x\\cos\\theta + m_y\\sin\\theta\\sin\\phi + m_z\\sin\\theta\\cos\\phi \\\\ |
|
m_y\\cos\\phi - m_z\\sin\\phi \\\\ |
|
-m_x\\sin\\theta + m_y\\cos\\theta\\sin\\phi + m_z\\cos\\theta\\cos\\phi |
|
\\end{bmatrix} |
|
\\end{array} |
|
|
|
Where :math:`\\mathbf{m}=\\begin{bmatrix}m_x & m_y & m_z\\end{bmatrix}^T` is |
|
the *normalized* vector of the measured magnetic field, which means |
|
:math:`\\|\\mathbf{m}\\|=1`. |
|
|
|
The yaw angle :math:`\\psi` is the tilt-compensated heading angle relative to |
|
magnetic North, computed as [FS-AN4248]_: |
|
|
|
.. math:: |
|
\\begin{array}{ll} |
|
\\psi &= \\mathrm{arctan2}(-b_y, b_x) \\\\ |
|
&= \\mathrm{arctan2}\\big(m_z\\sin\\phi - m_y\\cos\\phi, \\; m_x\\cos\\theta + \\sin\\theta(m_y\\sin\\phi + m_z\\cos\\phi)\\big) |
|
\\end{array} |
|
|
In the Attitude From Tilt, the documentation assigns roll=θ, and pitch=φ. However, in calculation of ψ the roll and pitch symbols have been swapped. This makes sense, since the original NXP documentation assigns the symbols this way - and was likely an error when translating the equation.
The code is still correct, however the documentation should be updated to match.
ahrs/ahrs/filters/tilt.py
Lines 47 to 78 in 48ed893