APIs description¶

Doxygen complete API documentation¶

Doxygen documentation Link to doxygen API documentation

Available classes¶

SRX_MODEL_INS_10_DOF¶

class SRX_MODEL_INS_10_DOF¶

Public Types

enum class 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

Public Functions

void step()¶

Run one step of the model.

This function has to be called at the rate of the model (0.001s)

void initialize()¶: Initialize the model and interfaces.

void setInputs(const modelInputs &inputs)¶

Set the Inputs used by the model.

Parameters:: inputs – is the struct containing the inputs

const modelOutputs &getOutputs()¶

Get the Outputs of the model.

Returns:: the struct containing the outputs

inline modelParameters &getParameters()¶

Get the Parameters of the model.

Returns:: the struct containing the references to parameters

struct modelInputs¶

This structure contains the inputs of the model.

This structure has to be at least partially filled and passed to the model at each step.

Public Members

std::array<float, 3> gyroDpsRate = {{0.F, 0.F, 0.F}}¶: Gyro xyz rates in deg/s.

std::array<float, 3> acceleroG = {{0.F, 0.F, 0.F}}¶: Accelerometer xyz in G.

std::array<float, 3> magGauss = {{0.F, 0.F, 0.F}}¶: Magnetometer xyz in Gauss.

float barometerAltitudeAmsl = 0.F¶: Barometer altitude above mean sea level in meters.

float barometerAltitudeRel = 0.F¶: Barometer altitude relative to the ground in meters.

uint64_t barometerTimestampUs = 0U¶: Timestamp of the barometer in microseconds.

uint64_t magTimestampUs = 0U¶: Timestamp of the magnetometer in microseconds.

float gpsHeadingAccuracy = 0.F¶: Heading accuracy of the GPS in radians.

float gpsMotionHeading = 0.F¶: Motion direction (motion heading) in radians.

uint8_t gpsDateValid = 0U¶: Validity of the GPS date.

double degGpsLatitude = 0.0¶: Latitude of the GPS location in degrees.

double degGpsLongitude = 0.0¶: Longitude of the GPS location in degrees.

float gpsHeightAboveSea = 0.F¶: Height of the GPS location above sea level in meters.

float gpsHorizontalAccuracy = 0.F¶: Horizontal accuracy of the GPS location.

float gpsVerticalAccuracy = 0.F¶: Vertical accuracy of the GPS location.

uint8_t gpsFixType = 0U¶

Type of GPS fix.

0: no fix, 1: Dead reckoning, 2: 2D fix, 3: 3D fix, 4: GNSS + dead reckoning

uint8_t gpsHour = 0U¶: Current hour according to the GPS time.

uint8_t gpsMinutes = 0U¶: Current minute according to the GPS time.

uint8_t gpsMonth = 0U¶: Current month according to the GPS time.

int32_t gpsNanoSeconds = 0¶: Current nanoseconds according to the GPS time.

uint8_t gpsNumSatellites = 0U¶: Number of satellites the GPS is currently connected to.

uint8_t gpsSeconds = 0U¶: Current second according to the GPS time.

uint16_t gpsYear = 0U¶: Current year according to the GPS time.

uint8_t gpsDay = 0U¶: Current day according to the GPS time.

float gpsSpeedAccuracy = 0.F¶: Accuracy of the GPS speed in m/s.

float gpsSpeedGround = 0.F¶: Ground speed of the GPS in m/s.

std::array<float, 3> gpsSpeedNED = {{0.F, 0.F, 0.F}}¶: North-East-Down (NED) speed of the GPS in m/s.

uint64_t gpsTimestampUs = 0U¶: Timestamp of the GPS in microseconds.

uint64_t timestampUs = 0U¶: Timestamp of the caling processor in microseconds.

bool reset = false¶: Reset the model (set to true for one step)

struct modelOutputs¶

This struct contains the outputs of the model.

Public Members

const float *localToBodyQuat = nullptr¶: Quaternion discribing body orientation relative to local frame.

Note

The quaternion is not globally aligned with NED with the magnetometer. It is indeed much more reliable than the one from localToGlobalQuat.

const float *nedToBodyQuat = nullptr¶: Quaternion discribing body orientation relative to global frame (NED)

Warning

The quaternion is globally aligned with NED with the magnetometer. It is subject to magnetometer disturbances and noise, especially if the sensor has not been calibrated properly.

const float *localToGlobalQuat = nullptr¶

Quaternion linking local frame to global frame (NED) with the relation quatGlobal = localToGlobalQuat x quatLocal.

The quaternion is globally aligned with NED with the magnetometer. It is subject to magnetometer disturbances and noise, especially if the sensor has not been calibrated properly.

const float *yawPitchRollLocal = nullptr¶

Yaw, pitch and roll in radians in local frame (yaw initialized to 0 at startup)

This information is obtained from the decomposition of localToBodyQuat using ZYX decomposition:

yaw is defined between -pi and pi
pitch is defined between -pi/2 and pi/2
roll is defined between -pi and pi.

Note

Yaw is not aligned with magnetometer and only correct slightly from it. The informations from this fields are indeed much more reliable than the ones from yawPitchRollGlobal.

const float *yawPitchRollGlobal = nullptr¶

Yaw, pitch and roll in radians in global frame (NED)

Yaw is aligned with magnetometer and does not include declination that has to be added to get a correct alignment with north. This information is obtained from the decomposition of nedToBodyQuat using ZYX decomposition:

yaw is defined between -pi and pi
pitch is defined between -pi/2 and pi/2
roll is defined between -pi and pi.

Warning

As the magnetometer is used as main source for heading, the yaw component is subject to magnetometer disturbances and noise, especially if the sensor has not been calibrated properly.

const float *tiltAngle = nullptr¶: Tilt angle in radians.

const float *yawLocalOffsetted = nullptr¶: Yaw angle offsetted to match global yaw (oriented to north) in radians.

const float *agYawLocalUnwrapped = nullptr¶: Yaw angle unwrapped in radians. Dimension size is 2 : [yawLocalUnwrapped yawLocalOffsettedUnwrapped].

const float *dstAltitude = nullptr¶: Altitude above ground in meters.

const float *spdUp = nullptr¶: Up speed in m/s.

const float *bodyRates = nullptr¶: Body rates (unbiased gyro rates) in radians/s.

const float *gyroBias = nullptr¶: Gyro bias in radians/s.

const float *accelerometerUnbiased = nullptr¶: Accelerometer unbiased in G.

const float *accelerometerZBias = nullptr¶: Accelerometer bias in G.

const float *linearAccelerationLocal = nullptr¶: Linear acceleration in m/s^2 in local frame.

const float *barometerAltitudeCorrected = nullptr¶: Barometer altitude corrected in meters.

const float *barometerBias = nullptr¶: Barometer bias in meters.

const uint8_t *immobility = nullptr¶: 1 if the INS is immobile

const uint8_t *reset = nullptr¶: 1 if the INS has been reset

const float *modelTimer = nullptr¶: Timer of the model in seconds.

const float *modelTimerFromReset = nullptr¶: Timer of the model in seconds since the last reset.

const float *klmMagStates = nullptr¶

Magnetic field estimator states (x6)

The states are [magFieldX magFieldY magFieldZ hardIronX hardIronY hardIronZ]

const float *rawMagNormFiltered = nullptr¶: Raw Magnetic Field Filtered Norm in Gauss. Dimension size is 2 : [normMagHorizontal normMagVertical].

const float *vertMagNormThreshold = nullptr¶: Current vertical magnetic field norm acceptation threshold. Dimension is 2 :[min max].

const float *horizMagNormThreshold = nullptr¶: Current horizontal magnetic field norm acceptation threshold. Dimension is 2 :[min max].

INT_HEADING_STATES headingState = INT_HEADING_STATES::unknown ¶: Current magnetic field estimation state.

const float *gaussMagHorizNorm = nullptr¶: Current estimated magnetic field horizontal norm.

const float *gaussMagVertNorm = nullptr¶: Current estimated magnetic field vertical norm.

const uint8_t *localWmmInfosSet = nullptr¶: Word Magnetic Model informations has been set at least one time and can be used to reject incorrect magnetic measurements.

const uint8_t *wmmUpdate = nullptr¶: World Magnetic Model informations updated.

const float *gaussWmmMagRefHorizNorm = nullptr¶: World Magnetic Model reference horizontal norm for the current position.

const float *gaussWmmMagRefNorm = nullptr¶: World Magnetic Model reference norm for the current position.

const float *gaussWmmMagRefNorthEastDown = nullptr¶

World Magnetic Model reference magnetic field values in Gauss for the current position.

The values are [North East Down] and are given in gauss

const float *gaussWmmMagRefVertNorm = nullptr¶: World Magnetic Model reference vertical norm for the current position.

const float *gpsDyear = nullptr¶: Computed decimal year from GPS date.

const float *wmmDeclination = nullptr¶: Magnetic declination in radians.

const float *wmmInclination = nullptr¶: Magnetic inclination in radians.

struct modelParameters¶

This struct contains the parameters of the model.

This structure has to be filled before calling the initialize function.

Public Members

float *ACCPROC_lowPassFilterDen = nullptr¶: Accelerometer low pass filter denominator coefficients (6th order, 7 coefficients)

float *ACCPROC_lowPassFilterNum = nullptr¶: Accelerometer low pass filter numerator coefficients (6th order, 7 coefficients)

float *ACCPROC_lowPassFilterInitGain = nullptr¶: Accelerometer low pass filter initial gain (initGain = 1 / sum(den))

float *GYROPROC_lowPassFilterDen = nullptr¶: Gyroscope low pass filter denominator coefficients (6th order, 7 coefficients)

float *GYROPROC_lowPassFilterNum = nullptr¶: Gyroscope low pass filter numerator coefficients (6th order, 7 coefficients)

float *GYROPROC_lowPassFilterInitGain = nullptr¶: Gyroscope low pass filter initial gain (initGain = 1 / sum(den))

float *MAGPROC_lowPassFilterNum = nullptr¶: Magnetometer low pass filter denominator coefficients (4th order, 5 coefficients)

float *MAGPROC_lowPassFilterDen = nullptr¶: Magnetometer low pass filter numerator coefficients (4th order, 5 coefficients)

float *MAGPROC_lowPassFilterInitGain = nullptr¶: Magnetometer low pass filter initial gain (initGain = 1 / sum(den))

float *BAROPROC_lowPassFilterNum = nullptr¶: Barometer low pass filter numerator coefficients (4th order, 5 coefficients)

float *BAROPROC_lowPassFilterDen = nullptr¶: Barometer low pass filter denominator coefficients (4th order, 5 coefficients)

float *BAROPROC_lowPassFilterInitGain = nullptr¶: Barometer low pass filter initial gain (initGain = 1 / sum(den))

float *BAROPROC_glitchLPFilterDen = nullptr¶: Barometer glitch low pass filter denominator coefficients (1st order, 2 coefficients)

float *BAROPROC_glitchLPFilterNum = nullptr¶: Barometer glitch low pass filter numerator coefficients (1st order, 2 coefficients)

float *BAROPROC_glitchLPFInitGain = nullptr¶: Barometer glitch low pass filter initial gain (initGain = 1 / sum(den))

float *BAROPROC_dstMaxThresholdNoise = nullptr¶

Barometer max threshold noise in meters.

If abs(baro-baro_glitch_filtered) > dstMaxThresholdNoise, the barometer is considered as glitched and rejected

float *DET_dpsImmoDisableThreshold = nullptr¶: Above this gyro rate difference, the immobility is set to false.

float *DET_dpsImmoDisableAbsGyro = nullptr¶: Above this absolute gyro rate on any axis, the immobility is set to false.

float *DET_dpsFiltImmoEnableThreshold = nullptr¶: Below this gyro rate difference, the immobility is set to true after some time.

float *DET_immoGyroLowPassTimeConstant = nullptr¶: Time constant of the low pass filter used to compute the gyro rate difference.

float *DET_immoTimeConfirmation = nullptr¶: Time during which the gyro rate difference has to be below DET_dpsFiltImmoEnableThreshold to set the immobility to true.

float *KLM_P0mat = nullptr¶: Initial covariance matrix (8x8)

float *KLM_Rmat = nullptr¶: Measurement noise covariance matrix (7x7)

float *KLM_Qmat = nullptr¶: Process noise covariance matrix (8x8)

float *KLM_baroOnlyGainAngles = nullptr¶: Gain applied to thetaErrorVector state and biases when in baro only mode.

float *KLM_baroOnlySpdCovGain = nullptr¶: Covariance gain for Q(SpdZNed, SpdZNed) when in baro only mode.

float *KLM_baroOnlyBaroNoiseCovGain = nullptr¶: Covariance gain for Rmat(baro) when in baro only mode.

float *KLM_baroOnlyRotMoveDesensibilizationTable = nullptr¶

Desensibilization table for Qmat and Rmat when in baro only mode (3x3 table)

0	0.5	1	% Table axis: norm for [Wx Wy] body rate in rad/s
5	1	0.1	% gain for Rmat(baro)
1	10	100	% gain for Qmat(spdZ, spdZ)

float *KLM_gainBiasBarometer = nullptr¶: Gain used for barometer bias estimation in immobility mode.

float *KLM_sAngleInitDuration = nullptr¶: Duration of the angle adjustment after reset (or initialization)

float *KLM_angleInitCovInclinoNoiseGain = nullptr¶: Inclinometer noise covariance gain applied to Rmat for inclinometer corrections.

float *KLM_angleInitCovAngleGain = nullptr¶: Model angle covariance gain applied to Qmat for thetaErrorVector states.

float *KLM_gLinAccelLevelRejectionInclino = nullptr¶: If abs(1-norm(accelero)) is above this threshold, the inclino is rejected.

float *KLM_gNoiseDesensibilizationTable = nullptr¶

Desensibilization table for Qmat and Rmat when in presence of non gravitational acceleration (5x3 table)

0	0.2	0.75	% Table Axis: Accelerometer noise (1-norm(g))
1	400000*2	115000000	% Rmat, inclino noise desensibilization
1	1	2.85	% Qmat, theta angle errors desensibilization
1	0.002	0.1	% Qmat, bias desensibilization
1	0.01	0.02	% Increase baro angles/bias corrections (Qmat(spdZ, spdZ), Qmat(dstZ, dstZ)/gain)
1	1	1	% Reduce barometer noise to increase its corrections on angles
1	0.1	0.01	% With high level of noise, barometer will have more weight on angle and bias correction. We can reduce the correction on the bias with this parameter

float *KLM_immobilityInclinoCovAngleGain = nullptr¶: Gain applied to thetaErrorVector states covariances when in immobility mode.

float *KLM_immobilityInclinoNoiseCovGain = nullptr¶: Gain applied to accelerometer Rmat covariances when in immobility mode.

float *KLM_inclinoNoiseTau = nullptr¶: Time constant of the low pass filter used to compute the inclinometer noise (abs(1-norm(accelero)))

float *KLM_gainBiasZ = nullptr¶: Gain applied for the accelerometer bias Z estimation state when in immobility mode.

float *KLM_gainGlobalMagAngleCorrection = nullptr¶: Gain used to compute global yaw (directed to north)

float *KLM_gainLocalMagAngleCorrection = nullptr¶: Gain used to correct local yaw with magnetometer.

Note

The correction level should be low or zero to avoid magnetometer noise impact on local estimation

float *KLM_headingJumpVal = nullptr¶: If the difference between the current global yaw and new magnetometer heading is above this threshold, a jump will be applied to the global yaw.

float *KLM_localMagAngleMaxVal = nullptr¶: Maximum value of the local yaw angle correction.

float *KLM_yawLocalOffsetAdjustmentGain = nullptr¶

This gain is used to adjust the local yaw offseted the global yaw.

The higher the gain, the faster the local yaw offsetted will be adjusted to the global yaw. The default value is 1e-4

uint8_t *KLMAG_bUseMagEst = nullptr¶: True if the magnetic field estimation is used.

float *KLMAG_P0mat = nullptr¶: Initial covariance matrix (6x8)

float *KLMAG_Rmat = nullptr¶: Measurement noise covariance matrix (3x3)

float *KLMAG_Qmat = nullptr¶: Process noise covariance matrix (6x6)

float *KLMAG_covHardIronReliable = nullptr¶: Hard Iron Estimate Covariance threshold to consider it reliable.

float *KLMAG_covMagFieldReliable = nullptr¶: Magnetic Field Estimate Covariance threshold to consider it reliable.

float *KLMAG_magHorizNormMinMax = nullptr¶: Initial magnetic field horizontal norm threshold (min/max) to consider the magnetic field as reliable (size 2 for [min max])

float *KLMAG_magVertNormMinMax = nullptr¶: Initial magnetic field vertical norm threshold (min/max) to consider the magnetic field as reliable (size 2 for [min max])

float *KLMAG_magVertNormThresold = nullptr¶

Magnetic field horizontal norm tolerance threshold to consider the magnetic field as reliable. The vector if of size 2 for a comparison vs [mean WorldMagneticModel].

Two modes are available to fix magnetic field thresholds. If World Magnetic Model is available (given by valid GPS position), the thresholds are computed as follow: threshold = mean WorldMagneticModel +/- tolerance. If World Magnetic Model is not available, the thresholds are computed as follow: threshold = mean magField +/- tolerance, with the meanMagField being considered after Kalman has converged. The tolerance should be smaller if world magnetic model is available as the information is more reliable than using the mean of estimated magnetic field

float *KLMAG_magHorizNormThresold = nullptr¶

Magnetic field vertical norm tolerance threshold to consider the magnetic field as reliable. The vector if of size 2 for a comparison vs [mean WorldMagneticModel].

float *KLMAG_magNormFilterUpdateRate = nullptr¶: For magnetic field validation, we use filtered norm. This parameter is used as follow: magNormFiltered = KLMAG_magNormFilterUpdateRate * rawMagNorm + (1-KLMAG_magNormFilterUpdateRate) * magNormFiltered. At 100Hz, a value of 0.1 is a good compromise between reactivity and noise rejection.

uint32_t *KLMAG_magReliableCounterThreshold = nullptr¶: When magnetometer is rejected because of disturbance detection, we consider it to become valid again after the count of valid values reaches this threshold. The default value is 500 which is 5 seconds at 100Hz.