.. _how-it-works-heading:
How it works: heading
=====================
.. toctree::
:maxdepth: 3
:caption: Contents:
Heading estimation is done using gyrometer integration, magnetometer measures, GPS speed vector direction (soon) and
the informations from World Magnetic Model (WMM) when correct informations are provided.
Magnetometer processing
-----------------------
Magnetometer calibration
~~~~~~~~~~~~~~~~~~~~~~~~
For the sensor to operate correctly, it is necessary to compensate soft iron and hard iron effects. They are generated by the environment and the system itself, close to the sensor. **This calibration is mandatory if you want to use the magnetometer.**
Calibration procedure should be done away from huge metallic objects (cars...) and from magnetic fields (power lines...).
A pseudo calibration code is included in the MMC5983MA library proposed. For application that do not require a high precision, this calibration should be enough.
For more precision, a more advanced calibration should be performed.
.. note::
A more complete calibration procedure using logged magnetometer data is available at `Magnetometer calibration `_ .
Magnetic field and hard iron observation
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Onboard estimator is included to estimate the magnetic field and hard iron effects. It has its limits and can not replace the static calibration.
Magnetometer ideal measurement should be the rotation in the horizontal plane of the local magnetic field vector by the sequence: heading, followed by Pitch
and Roll rotations.
The transformation gives:
.. math::
B_{{Ned}_{WMM}} = R_{BodyToNed}(t) \cdot (B_{{Body}_{mag}} - B_{{Body}_{hardIron}})
With the bias and the local magnetic field being time constants, the derivation gives:
.. math::
\frac{d B_{{Ned}_{WMM}}}{dt} = 0 = \frac{d R_{BodyToNed}}{dt} \cdot (B_{{Body}_{mag}} - B_{{Body}_{hardIron}}) + R_{BodyToNed} \cdot \frac{d (B_{{Body}_{mag}} - B_{{Body}_{hardIron}})}{dt}
\\
R_{BodyToNed}(t) \cdot \dot{B}_{{Body}_{mag}} = - R_{BodyToNed}(t) \cdot \begin{bmatrix} 0 & -\omega_z & -\omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix} \cdot (B_{{Body}_{mag}} - B_{{Body}_{hardIron}})
Which give:
.. math::
\begin{cases}
\dot{B}_{{Body}_{mag}} = \begin{bmatrix} 0 & -\omega_z & -\omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix} \cdot (B_{{Body}_{mag}} - B_{{Body}_{hardIron}}) \\
\dot{B}_{{Body}_{hardIron}} = 0
\end{cases}
This model can be used to predict Magnetic field vector and use correction step with magnetometer measures.
Thresholds Setting
~~~~~~~~~~~~~~~~~~~~~
When the Kalman estimator convergence is reached (low states covariances), we set acceptable magnetic field norm thresholds in order to reject magnetic disturbances. This thresholds acceptances are accessible through :cpp:member:`SRX_MODEL_INS_10_DOF::modelParameters::KLMAG_magVertNormThresold`,
and :cpp:member:`SRX_MODEL_INS_10_DOF::modelParameters::KLMAG_magHorizNormThresold`
With World Magnetic model available
""""""""""""""""""""""""""""""""""""
When the World Magnetic Model is available, the norm thresholds are based on the WMM model with smaller margins for :cpp:member:`SRX_MODEL_INS_10_DOF::modelParameters::KLMAG_magVertNormThresold` and :cpp:member:`SRX_MODEL_INS_10_DOF::modelParameters::KLMAG_magHorizNormThresold`. If the measured magnetic field is not
consistent with the WMM model, measures are rejected.
Without World Magnetic model
""""""""""""""""""""""""""""
Without the WMM model, the thresholds are set to estimated magnetic field norm after kalman convergence and readapted during time.
Margins are larger than with WMM infos.
Outputs to consider
~~~~~~~~~~~~~~~~~~~~
Enums
""""""
- :cpp:member:`SRX_MODEL_INS_10_DOF::modelOutputs::headingState`: The heading state indicates the reliability of the heading estimation. It is based on Kalman Filter covariances and if the world magnetic model is used or not. It considers also the magnetic field norms in order to reject magnetic disturbances. The states are:
.. doxygenenum:: SRX_MODEL_INS_10_DOF::INT_HEADING_STATES
.. tip::
To converge, Kalman filter needs to be able to match magnetometer measures with gyrometer angular rates. To make it converge faster,
after startup, it is recommended to make the system rotate around all the axis, like a magnetometer calibration and put it immobile when
the heading state is OK.
Heading values
""""""""""""""
Depending on the application, the following outputs can be used:
+-------------------+------------------------------------+------------------------+------------+---------------+-----------------------------+
| Name | Description | Absolute Precision | Low Noise | Reliability | Usage |
| | +------------+-----------+ | | |
| | | Short term | Long term | | | |
+===================+====================================+============+===========+============+===============+=============================+
| yawPitchRollLocal | Relative information integrated | 0 | 0 | +++ | +++ | For control. If Absolute |
| | From startup. It suffers no | | | | | heading is not mandatory, |
| | discontinuity | | | | | this is the best information|
+-------------------+------------------------------------+------------+-----------+------------+---------------+-----------------------------+
| yawPitchRolGlobal | Heading is aligned on absolute yaw.| \+ | +++ | \+ | \+ | For estimation. If Absolute |
| | It uses estimated magnetic field | | | | | heading is mandatory. |
| | and main kalman filter for | | | | | |
| | propagation | | | | | |
+-------------------+------------------------------------+------------+-----------+------------+---------------+-----------------------------+
| yawLocalOffsetted | Local yaw, offseted To match global| +++ | ++ | +++ | ++ | For control. If Absolute |
| | when information is considered | | | | | heading is mandatory. |
| | reliable. It is slowly corrected | | | | | |
| | by global yaw. It suffers | | | | | |
| | discontinuity when headingState | | | | | |
| | becomes reliable the first time | | | | | |
+-------------------+------------------------------------+------------+-----------+------------+---------------+-----------------------------+
Unwrapped variations of yawLocal and yawLocalOffseted are available in the outputs. This outputs do not suffer -pi/pi discontinuity:
- :cpp:member:`SRX_MODEL_INS_10_DOF::modelOutputs::agYawLocalUnwrapped`
World Magnetic Model usage
---------------------------
The World Magnetic Model (WMM) is the standard model for navigation, attitude, and heading referencing systems using the geomagnetic field
Sysrox code integrates this model and can use it to improve the heading estimation.
If the current position is known, as well as the date, the WMM can be used to estimate the theoretical magnetic field at this position.
This is useful in the first place to get the declination for the current location in order to convert the magnetic heading to the true heading.
This can be also used to reject magnetic interferences that are not consistent with the WMM model. If any World Magnetic Information is available at
any time, the heading estimation will consider these informations priorily for disturbances rejection instead of the estimated validation tresholds.
How to configure
~~~~~~~~~~~~~~~~
If GPS informations are available, injecting these in the INS inputs will allow WMM settings to be used for heading estimation.
Without the model, if location is known, it can be set-up during execution:
.. admonition:: Example
For example the current inputs can be set to configure the WMM model without GPS:
.. code-block:: cpp
:linenos:
insInputs.gpsTimestampUs = 1; // Any value
insInputs.degGpsLatitude = 48.5586127;
insInputs.degGpsLongitude = 3.3011296;
insInputs.gpsYear = 2024;
insInputs.gpsMonth = 3;
ins.setInputs(insInputs);
To know if the WMM is used, the following informations can be used:
- :cpp:member:`SRX_MODEL_INS_10_DOF::modelOutputs::localWmmInfosSet`: True if the WMM is used
- :cpp:member:`SRX_MODEL_INS_10_DOF::modelOutputs::gpsDyear`: Different than 0 if the WMM is used
- ...