How it works: heading

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:

\[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:

\[ \begin{align}\begin{aligned}\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}\\\begin{split}\\\end{split}\\\begin{split}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}})\end{split}\end{aligned}\end{align} \]

Which give:

\[\begin{split}\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}\end{split}\]

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 SRX_MODEL_INS_10_DOF::modelParameters::KLMAG_magVertNormThresold, and 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 SRX_MODEL_INS_10_DOF::modelParameters::KLMAG_magVertNormThresold and 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

  • 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:

enum class SRX_MODEL_INS_10_DOF::INT_HEADING_STATES : uint32_t

Enumeration representing the different states of the heading through magnetic field estimation.

Values:

enumerator unknown

Unknown heading state

enumerator disturbed_dead_reckoning

Magnetic field and hard irons have been correctly estimated but magnetometer is actually considered unreliable

enumerator unprecise_with_wmm

World magnetic model reference has been set but hard irons have not been correctly estimated and magnetometer is considered unreliable.

enumerator precise_no_wmm

Heading state is considered reliable because of correct magnetometer measurements thresholds have been set without World Magnetic Model (WMM)

enumerator precise_wmm

Heading state is considered reliable because of correct magnetometer measurements thresholds have been set with World Magnetic Model (WMM) and are more precise than the automatically one sets

enumerator precise_wmm_gps

Heading state is considered reliable because of correct magnetometer measurements thresholds have been set with World Magnetic Model (WMM) and are more precise than the automatically one sets . Heading is GPS aided

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 From startup. It suffers no discontinuity

0

0

+++

+++

For control. If Absolute heading is not mandatory, this is the best information

yawPitchRolGlobal

Heading is aligned on absolute yaw. It uses estimated magnetic field and main kalman filter for propagation

+

+++

+

+

For estimation. If Absolute heading is mandatory.

yawLocalOffsetted

Local yaw, offseted To match global when information is considered reliable. It is slowly corrected by global yaw. It suffers discontinuity when headingState becomes reliable the first time

+++

++

+++

++

For control. If Absolute heading is mandatory.

Unwrapped variations of yawLocal and yawLocalOffseted are available in the outputs. This outputs do not suffer -pi/pi discontinuity:

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:

Example

For example the current inputs can be set to configure the WMM model without GPS:

1insInputs.gpsTimestampUs  = 1; // Any value
2insInputs.degGpsLatitude  = 48.5586127;
3insInputs.degGpsLongitude = 3.3011296;
4insInputs.gpsYear  = 2024;
5insInputs.gpsMonth = 3;
6
7ins.setInputs(insInputs);

To know if the WMM is used, the following informations can be used: