function

Tests.EstimatorHealthTests.mocapTick

One estimator tick against a mocap-only fixture at a fixed sample rate

Public declarations

Explicit source declarations; dimensions, modifications and defaults are shown as written, without evaluation.

Role	Name	Type	Dimensions / attributes / default
input	`initialized`	`Boolean`
input	`position`	`Real`	`[3]`
input	`velocity`	`Real`	`[3]`
input	`quaternion`	`Real`	`[4]`
input	`gyroscopeBias`	`Real`	`[3]`
input	`accelerometerBias`	`Real`	`[3]`
input	`covariance`	`Estimation.StrapdownINS.ESKF.Covariance`
input	`rejections`	`Integer`
input	`rejectionElapsed_s`	`Real`
input	`mocapPosition`	`Real`	`[3]`
input	`timestamp_s`	`Real`
input	`dt`	`Real`
input	`innovationGate`	`Real`
input	`covarianceInflateWindow_s`	`Real`
input	`aidingDivergentWindow_s`	`Real`
input	`covarianceInflateTimeConstant_s`	`Real`
input	`aidingStaleTimeout_s`	`Real`
input	`anchorSourcePrevious`	`Integer`
output	`positionNext`	`Real`	`[3]`
output	`velocityNext`	`Real`	`[3]`
output	`quaternionNext`	`Real`	`[4]`
output	`gyroscopeBiasNext`	`Real`	`[3]`
output	`accelerometerBiasNext`	`Real`	`[3]`
output	`covarianceNext`	`Estimation.StrapdownINS.ESKF.Covariance`
output	`initializedNext`	`Boolean`
output	`rejectionsNext`	`Integer`
output	`rejectionElapsedNext_s`	`Real`
output	`mocapAccepted`	`Boolean`
output	`recoveryStage`	`Integer`
output	`correctionOutcome`	`Integer`
output	`estimateValid`	`Boolean`

Modelica declarations and implementation

Show parameters, interfaces and source

Parameters, inputs, outputs, defaults and equations from the source file.

function mocapTick
    "One estimator tick against a mocap-only fixture at a fixed sample rate"
    input Boolean initialized;
    input Real position[3];
    input Real velocity[3];
    input Real quaternion[4];
    input Real gyroscopeBias[3];
    input Real accelerometerBias[3];
    input Estimation.StrapdownINS.ESKF.Covariance covariance;
    input Integer rejections;
    input Real rejectionElapsed_s;
    input Real mocapPosition[3];
    input Real timestamp_s;
    input Real dt;
    input Real innovationGate;
    input Real covarianceInflateWindow_s;
    input Real aidingDivergentWindow_s;
    input Real covarianceInflateTimeConstant_s;
    input Real aidingStaleTimeout_s;
    input Integer anchorSourcePrevious;
    output Real positionNext[3];
    output Real velocityNext[3];
    output Real quaternionNext[4];
    output Real gyroscopeBiasNext[3];
    output Real accelerometerBiasNext[3];
    output Estimation.StrapdownINS.ESKF.Covariance covarianceNext;
    output Boolean initializedNext;
    output Integer rejectionsNext;
    output Real rejectionElapsedNext_s;
    output Boolean mocapAccepted;
    output Integer recoveryStage;
    output Integer correctionOutcome;
    output Boolean estimateValid;
  protected
    Boolean predictionOk;
    Boolean gpsPositionOk;
    Boolean gpsVelocityOk;
    Boolean magnetometerOk;
    Boolean barometerOk;
    Boolean flowOk;
    Integer correctionSource;
    Real correctionNis;
    Integer mocapRejections;
    Integer gpsRejections;
    Integer flowRejections;
    Real imuOmegaHeld[3];
    Real imuAccelHeld[3];
    Real imuStampHeld;
    Boolean imuHeldFlag;
    Boolean reseeded;
    Integer anchorSourceNext;
    Real mocapStaleNext_s;
    Real gpsStaleNext_s;
    Real flowStaleNext_s;
    Real mocapTimestampConsumedNext_s;
    Real gpsTimestampConsumedNext_s;
    Real magnetometerTimestampConsumedNext_s;
    Real barometerTimestampConsumedNext_s;
    Real opticalFlowTimestampConsumedNext_s;
  algorithm
    // Level flight: the specific force exactly cancels gravity, so an
    // accepted prediction leaves the nominal state where it was and every
    // position change below is attributable to the aiding path alone.
    (positionNext,
     velocityNext,
     quaternionNext,
     gyroscopeBiasNext,
     accelerometerBiasNext,
     covarianceNext,
     initializedNext,
     predictionOk,
     mocapAccepted,
     gpsPositionOk,
     gpsVelocityOk,
     magnetometerOk,
     barometerOk,
     flowOk,
     rejectionsNext,
     rejectionElapsedNext_s,
     recoveryStage,
     correctionOutcome,
     correctionSource,
     correctionNis,
     estimateValid,
     reseeded,
     mocapRejections,
     gpsRejections,
     flowRejections,
     anchorSourceNext,
     mocapStaleNext_s,
     gpsStaleNext_s,
     flowStaleNext_s,
     imuOmegaHeld,
     imuAccelHeld,
     imuStampHeld,
     imuHeldFlag,
     mocapTimestampConsumedNext_s,
     gpsTimestampConsumedNext_s,
     magnetometerTimestampConsumedNext_s,
     barometerTimestampConsumedNext_s,
     opticalFlowTimestampConsumedNext_s) :=
      Estimation.StrapdownINS.ESKF.step(
      initialized,
      Estimation.StrapdownINS.ESKF.State(
        barometerBiasCrossCovariance=zeros(15),
        barometerBias_m=0.0, barometerBiasVariance_m2=0.0, useJointBarometerBias=false,
        positionWorldEnu_m=position,
        velocityWorldEnu_m_s=velocity,
        quaternionWorldBody=quaternion,
        gyroscopeBiasBodyFlu_rad_s=gyroscopeBias,
        accelerometerBiasBodyFlu_m_s2=accelerometerBias,
        covariance=covariance,
        useSquareRootCovariance=false, covarianceRoot=zeros(15, 15)),
      false,
      Avionics.ImuSample(
        valid=true,
        fresh=true,
        timestamp_s=timestamp_s,
        angularVelocityBodyFlu_rad_s=zeros(3),
        specificForceBodyFlu_m_s2={0.0, 0.0, 9.81},
        deltaAngleBodyFlu_rad=zeros(3),
        deltaVelocityBodyFlu_m_s={0.0, 0.0, 0.00981},
        deltaPositionBodyFlu_m={0.0, 0.0, 4.905e-6},
        deltaQuaternionBodyFlu={1.0, 0.0, 0.0, 0.0},
        gyroscopeBiasLinearizationBodyFlu_rad_s=zeros(3),
        accelerometerBiasLinearizationBodyFlu_m_s2=zeros(3),
        deltaRotationGyroscopeBiasJacobian_s=-identity(3) * 0.001,
        deltaVelocityGyroscopeBiasJacobian_m=zeros(3, 3),
        deltaVelocityAccelerometerBiasJacobian_s=-identity(3) * 0.001,
        deltaPositionGyroscopeBiasJacobian_m_s=zeros(3, 3),
        deltaPositionAccelerometerBiasJacobian_s2=
          -0.5 * identity(3) * 1.0e-6,
        integrationTime_s=0.001),
      Avionics.MocapSample(
        valid=true,
        fresh=true,
        timestamp_s=timestamp_s,
        positionWorldEnu_m=mocapPosition,
        quaternionWorldBody={1.0, 0.0, 0.0, 0.0},
        positionCovarianceWorld_m2=identity(3) * 1.0e-4,
        attitudeCovarianceBody_rad2=identity(3) * 1.0e-4),
      Avionics.GpsSample(
        valid=false,
        fresh=false,
        positionValid=false,
        velocityValid=false,
        timestamp_s=0.0,
        geodetic_deg_m=zeros(3),
        positionWorldEnu_m=zeros(3),
        velocityWorldEnu_m_s=zeros(3),
        positionCovarianceWorld_m2=identity(3),
        velocityCovarianceWorld_m2_s2=identity(3)),
      Avionics.MagnetometerSample(
        valid=false,
        fresh=false,
        timestamp_s=0.0,
        magneticFieldBodyFlu_T={18.0e-6, 4.0e-6, -47.0e-6},
        covarianceBody_T2=identity(3) * 1.0e-12),
      Avionics.BarometerSample(
        valid=false,
        fresh=false,
        timestamp_s=0.0,
        altitudeWorldEnu_m=0.0,
        variance_m2=1.0),
      Avionics.OpticalFlowSample(
        valid=false,
        fresh=false,
        timestamp_s=0.0,
        integratedLineOfSight_rad=zeros(2),
        integratedLineOfSightCovariance_rad2=identity(2),
        integratedGyroscopeBodyFlu_rad=zeros(3),
        integratedGyroscopeCovariance_rad2=identity(3),
        integrationTime_s=0.0,
        groundDistance_m=1.0,
        groundDistanceVariance_m2=0.01,
        quality=0.0),
      {0.0, 0.0, -9.81},
      dt,
      Estimation.StrapdownINS.ESKF.Tuning(
        initialState=Estimation.StrapdownINS.ESKF.NominalState(
          positionWorldEnu_m=zeros(3),
          velocityWorldEnu_m_s=zeros(3),
          quaternionWorldBody={1.0, 0.0, 0.0, 0.0},
          gyroscopeBiasBodyFlu_rad_s=zeros(3),
          accelerometerBiasBodyFlu_m_s2=zeros(3)),
        initialVariances=Estimation.StrapdownINS.InitialVariances(
          position_m2=fill(1.0, 3),
          velocity_m2_s2=fill(1.0, 3),
          attitude_rad2=fill(0.25, 3),
          gyroscopeBias_rad2_s2=fill(1.0e-4, 3),
          accelerometerBias_m2_s4=fill(1.0e-2, 3)),
        processNoise=Estimation.StrapdownINS.ProcessNoise(
          gyroscope_rad2_s=identity(3) * 1.0e-5,
          accelerometer_m2_s3=identity(3) * 1.0e-3,
          gyroscopeBias_rad2_s3=identity(3) * 1.0e-8,
          accelerometerBias_m2_s5=identity(3) * 1.0e-6),
        varianceLimits=Estimation.StrapdownINS.ESKF.VarianceLimits(
          position_m2=fill(1.0e4, 3),
          velocity_m2_s2=fill(4.0e2, 3),
          attitude_rad2=fill(10.0, 3),
          gyroscopeBias_rad2_s2=fill(1.0e-2, 3),
          accelerometerBias_m2_s4=fill(1.0, 3)),
        innovationGate=innovationGate,
        localMagneticFieldWorldEnu_T={18.0e-6, 4.0e-6, -47.0e-6},
        barometerBias_m=0.0,
        barometerBiasVariance_m2=1.0,
        maximumAidingDelay_s=0.25,
        minimumOpticalFlowQuality=0.2,
        minimumOpticalFlowGroundDistance_m=0.2,
        covarianceInflateWindow_s=covarianceInflateWindow_s,
        covarianceInflateTimeConstant_s=covarianceInflateTimeConstant_s,
        aidingDivergentWindow_s=aidingDivergentWindow_s,
        aidingStaleTimeout_s=aidingStaleTimeout_s,
        aidingReseedWindow_s=0.0,
        initialAlignmentWindow_s=0.0,
        initialAlignmentTimeout_s=0.0,
        quietSpecificForceTolerance_m_s2=0.5,
        quietAngularRateLimit_rad_s=0.1,
        quietFilterTimeConstant_s=0.0,
        pseudoPositionVariance_m2=0.0,
        zeroVelocityVariance_m2_s2=0.0),
      rejections,
      rejectionElapsed_s,
      0,
      0,
      0,
      anchorSourcePrevious,
      0.0,
      0.0,
      0.0,
      zeros(3),
      zeros(3),
      0.0,
      timestamp_s - dt,
      -1.0e30,
      -1.0e30,
      -1.0e30,
      -1.0e30,
      0.0,
      0.0,
      zeros(3),
      0,
      zeros(3),
      zeros(3));
  end mocapTick;