.. _how-it-works-tuning:
How it works: tuning
=====================
.. toctree::
:maxdepth: 3
:caption: Contents:
.. raw:: html
.. role:: fonttitle
.. |doxydoc| raw:: html
Model parameters
To adapt the INS to your system, the system if fully configurable with tunable parameters described in the API documentation:
- API reference: :cpp:struct:`SRX_MODEL_INS_10_DOF::modelParameters`, :cpp:func:`SRX_MODEL_INS_10_DOF::getParameters`
- Doxygen full documentation: |doxydoc|
For 2D dimension parameters, the table are counted as column major. For example, the 3x3 matrix :math:`\begin{bmatrix} 1 & 2 & 3 \\ 4 & 5 & 6 \\ 7 & 8 & 9 \end{bmatrix}` will be stored as [1, 4, 7, 2, 5, 8, 3, 6, 9].
.. admonition:: Tune parameters
For example, to change noise covariance matrix for Y accelerometer measurement, you can do:
.. code-block:: cpp
ins.getParameters().KLM_Rmat[8] = 5.F;
If you want to change gain for local yaw correction by magnetometer:
.. code-block:: cpp
*ins.getParameters().KLM_gainLocalMagAngleCorrection = 1e-3F;
The INS proposed used an extended Kalman filter. It already includes several mechanisms to adapt to many situations and the base configuration should be enough for most of the systems.
The possibility is given to override the default configuration at startup or during execuption.
Calibrations
------------
For the system to work properly, it is necessary to calibrate the sensors. Especially the magnetometer.
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 `_ .
Accelerometer calibration
~~~~~~~~~~~~~~~~~~~~~~~~~
The accelerometer is factory calibrated and only high precision application should require a calibration.
.. note::
A complete calibration procedure using logged accelerometer data is available at `Accelerometer calibration `_.
General kalman filter tuning
----------------------------
Measurement noise covariance matrix and process noise covariance matrix are both tunable.
.. admonition:: INS kalman states
.. math::
X_k = \left(\begin{array}{c} \mathbf{\theta x}_{\mathbf{err}}\\ \mathbf{\theta y}_{\mathbf{err}}\\ \mathbf{\theta z}_{\mathbf{err}}\\ b_{\mathbf{x}}\\ b_{\mathbf{y}}\\ b_{\mathbf{z}}\\ V_{\mathbf{NED}}\\ D_{\mathbf{NED}} \end{array}\right)
- Measurement noise covariance matrix (:math:`R_{mat}`) is the covariance of the measurement noise. It is used to weight the measurement in the Kalman filter. It is generaly a diagonal matrix as sensors noise are supposed gaussian and decorrelated. The diagonal elements are the variance of the measurement noise. It should be based on the real characterized sensor noise. In some situation, it is not the case because of some trade-off that have to be done (uncompensated reactions to disturbances, bad modelization...). The higher the variance, the less the measurement is trusted. The lower the variance, the more the measurement is trusted. The variable is accessible through :cpp:member:`SRX_MODEL_INS_10_DOF::modelParameters::KLM_Rmat`.
- Process noise covariance matrix (:math:`Q_{mat}`) is the covariance of the process noise. It should be based on the real characterized process noise. It traduces maximal error variance we consider for some state prediction for one time step. It is used to weight the prediction in the Kalman filter. The diagonal elements are the variance of the prediction noise. The higher the variance, the less the prediction is trusted (and the more we rely on sensors corrections). The lower the variance, the more the prediction is trusted. The variable is accessible through :cpp:member:`SRX_MODEL_INS_10_DOF::modelParameters::KLM_Qmat`.
Variable gains on these factors are accessible to mitigate the effects of the sensors depending on the situations.
Initialization
--------------
At startup, the attitude will be initialized with the accelerometer. To accelerate alignement with inclinometer angles:
- The measurement noise covariance related to the inclinometer is decreased by a factor of :cpp:member:`SRX_MODEL_INS_10_DOF::modelParameters::KLM_angleInitCovInclinoNoiseGain` to help relying more on the inclinometer.
- The process noise covariance related to the angle-axis rotation error vector if increased by a factor of :cpp:member:`SRX_MODEL_INS_10_DOF::modelParameters::KLM_angleInitCovAngleGain` to help relying less on our prediction and more on the inclinometer.
for a duration :cpp:member:`SRX_MODEL_INS_10_DOF::modelParameters::KLM_sAngleInitDuration`.
Adjusting these parameters can be done for a custom way to initialize the attitude.
Immobility
----------
During immobility:
- Accelerometer is considered highly reliable and its measurement noise covariance is decreased by a factor of :cpp:member:`SRX_MODEL_INS_10_DOF::modelParameters::KLM_immobilityInclinoNoiseCovGain`. The process noise covariance related to the angle-axis rotation error vector if increased by a factor of :cpp:member:`SRX_MODEL_INS_10_DOF::modelParameters::KLM_immobilityInclinoCovAngleGain` to help relying less on our prediction and more on the inclinometer.
- Barometer measures are considered to be biases with a model equal to a constant and a correction gain of :cpp:member:`SRX_MODEL_INS_10_DOF::modelParameters::KLM_gainBiasBarometer`
- Accelerometer Z bias is estimated with a model equal to a constant and a correction gain of :cpp:member:`SRX_MODEL_INS_10_DOF::modelParameters::KLM_gainBiasZ`
Sensors handling
----------------
Sensors have their strengths and weaknesses. Being able to weight them depending on the situation is mandatory to archive good results.
This is why several parameters are available to adjust the INS behavior depending on the operating points
Accelerometer handling
~~~~~~~~~~~~~~~~~~~~~~
Accelerometer is the INS main attitude correction source. Used as a inclinometer like explained in the section :ref:`Sensor usage accelerometer` , it supposes that the accelerometer measures only gravity acceleration. When the mobile is moved or sense non gravitational acceleration, angle measures are corrupted.
To mitigate this effect, several parameters are available. They are based on the difference between 1g and the accelerometer norm:
.. math::
noise = \lvert 1 - \sqrt{a_{\mathbf{x}}^2 + a_{\mathbf{y}}^2 + a_{\mathbf{z}}^2} \rvert
- This noise can be filtered with a first low pass filter with a time constant defined by :cpp:member:`SRX_MODEL_INS_10_DOF::modelParameters::KLM_inclinoNoiseTau`.
- Accerometer attitude corrections can be rejected above a certain noise threshold. This is the best solution if the INS is facing a sustain non gravitational acceleration. The threshold is defined by :cpp:member:`SRX_MODEL_INS_10_DOF::modelParameters::KLM_gLinAccelLevelRejectionInclino`.
- In reaction to noise variations, user can configure :cpp:member:`SRX_MODEL_INS_10_DOF::modelParameters::KLM_gNoiseDesensibilizationTable` based on the noise value. This way, inclinometer measurement noise can be increased or decreased by a gain. The same with process noise for angle corrections and bias estimation. This table is also used to increase the weight of the barometer for angle corrections. Even if it can help, this solution should be used with care as it will add noise to angle estimation and altitude estimation.
.. tip::
If the user do not want to use inclinometer at all, setting :cpp:member:`SRX_MODEL_INS_10_DOF::modelParameters::KLM_gLinAccelLevelRejectionInclino` to a negative value will disable the use of accelerometer as inclinometer.
Magnetometer handling
~~~~~~~~~~~~~~~~~~~~~~
Magnetometer impact on corrections on global and local estimations can be configured. The magnetometer is fused in a way only yaw is impacted on both quaternions (heavily for global estimation and lightly for local estimation). Pitch and roll will remain unaffected.
.. note::
Important rework of magnetometer handling will be released soon and will include disturbances online estimation, coherency test... This should greatly improve global heading estimation!
Global corrections
^^^^^^^^^^^^^^^^^^
Global yaw corrections will be configured throught :cpp:member:`SRX_MODEL_INS_10_DOF::modelParameters::KLM_gainGlobalMagAngleCorrection`. The bigger this gain, the more global yaw will follow magnetometer.
When the difference between global yaw and magnetometer yaw is too big, the global yaw will align with magnetometer using a "jump". The jump threshold is defined by :cpp:member:`SRX_MODEL_INS_10_DOF::modelParameters::KLM_headingJumpVal`.
Local corrections
^^^^^^^^^^^^^^^^^
The magnetometer can also corrects the yaw of the local quaternion. This correction is way smaller than its global counterpart: :cpp:member:`SRX_MODEL_INS_10_DOF::modelParameters::KLM_gainGlobalMagAngleCorrection`.
The maximal correction value can be clamped thanks to :cpp:member:`SRX_MODEL_INS_10_DOF::modelParameters::KLM_localMagAngleMaxVal`.
.. tip::
- In case user wants to avoid totally magnetometer corrections on local yaw, either of these parameters can be set to 0.
- The user that would like local heading offsetted to match global one (adjusted when INS is immobile) can use :cpp:member:`SRX_MODEL_INS_10_DOF::modelOuputs::yawLocalOffsetted`.
Barometer handling
~~~~~~~~~~~~~~~~~~~~
Barometer is used to correct vertical speed and altitude. With the link between :math:`V_{\mathbf{NED}}` and attitude during prediction step, it is also able to provide corrections for attitude estimation.
When inclinometer is not available (rejected for example), specific parameters allow to give more importance to barometer for attitude corrections:
- Barometer measure noise noise can be reduced with :cpp:member:`SRX_MODEL_INS_10_DOF::modelParameters::KLM_baroOnlyBaroNoiseCovGain`. This will increase the weight of barometer corrections for altitude and attitude estimations.
- Speed process noise can be decreased with :cpp:member:`SRX_MODEL_INS_10_DOF::modelParameters::KLM_baroOnlySpdCovGain`. This will increase the weight of barometer corrections for speed and attitude estimations, in addition to more noise on both estimates
- Angle-axis rotation error process noise can be increased with :cpp:member:`SRX_MODEL_INS_10_DOF::modelParameters::KLM_baroOnlyGainAngles`. This will increase the weight of barometer corrections for attitude estimations, in addition to more noise on angles.