How it works: tuning

To adapt the INS to your system, the system if fully configurable with tunable parameters described in the API documentation:

For 2D dimension parameters, the table are counted as column major. For example, the 3x3 matrix \(\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].

Tune parameters

For example, to change noise covariance matrix for Y accelerometer measurement, you can do:

ins.getParameters().KLM_Rmat[8] = 5.F;

If you want to change gain for local yaw correction by magnetometer:

*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.

INS kalman states

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

  • Measurement noise covariance matrix (\(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 SRX_MODEL_INS_10_DOF::modelParameters::KLM_Rmat.

  • Process noise covariance matrix (\(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 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:

for a duration 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:

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

\[noise = \lvert 1 - \sqrt{a_{\mathbf{x}}^2 + a_{\mathbf{y}}^2 + a_{\mathbf{z}}^2} \rvert\]

Tip

If the user do not want to use inclinometer at all, setting 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 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 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: SRX_MODEL_INS_10_DOF::modelParameters::KLM_gainGlobalMagAngleCorrection. The maximal correction value can be clamped thanks to 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 SRX_MODEL_INS_10_DOF::modelOuputs::yawLocalOffsetted.

Barometer handling

Barometer is used to correct vertical speed and altitude. With the link between \(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: