function

Estimation.StrapdownINS.ESKF.correctAbsoluteAidingTick

Select and fuse one absolute aiding sample without expanding the dispatcher

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`previous`	`State`
input	`imu`	`Avionics.ImuSample`
input	`mocap`	`Avionics.MocapSample`
input	`gps`	`Avionics.GpsSample`
input	`tuning`	`Tuning`
input	`gravityWorldEnu_m_s2`	`Real`	`[3]`
input	`predictionAccepted`	`Boolean`
input	`reseedNow`	`Boolean`
input	`mocapNew`	`Boolean`
input	`gpsNew`	`Boolean`
input	`anchorExclusive`	`Boolean`
input	`anchorPresent`	`Integer`
input	`imuTimestampHeldNext_s`	`Real`
input	`imuAngularVelocityHeldNext_rad_s`	`Real`	`[3]`
input	`imuSpecificForceHeldNext_m_s2`	`Real`	`[3]`
input	`mocapTimestampConsumedPrevious_s`	`Real`
input	`gpsTimestampConsumedPrevious_s`	`Real`
output	`working`	`State`
output	`mocapCorrectionAccepted`	`Boolean`
output	`gpsPositionCorrectionAccepted`	`Boolean`
output	`gpsVelocityCorrectionAccepted`	`Boolean`
output	`correctionAttempted`	`Boolean`
output	`correctionAccepted`	`Boolean`
output	`correctionOutcome`	`Integer`
output	`correctionSource`	`Integer`
output	`normalizedInnovationSquared`	`Real`
output	`mocapTimestampConsumedNext_s`	`Real`
output	`gpsTimestampConsumedNext_s`	`Real`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function correctAbsoluteAidingTick
  "Select and fuse one absolute aiding sample without expanding the dispatcher"
  input State previous;
  input Avionics.ImuSample imu;
  input Avionics.MocapSample mocap;
  input Avionics.GpsSample gps;
  input Tuning tuning;
  input Real gravityWorldEnu_m_s2[3];
  input Boolean predictionAccepted;
  input Boolean reseedNow;
  input Boolean mocapNew;
  input Boolean gpsNew;
  input Boolean anchorExclusive;
  input Integer anchorPresent;
  input Real imuTimestampHeldNext_s;
  input Real imuAngularVelocityHeldNext_rad_s[3];
  input Real imuSpecificForceHeldNext_m_s2[3];
  input Real mocapTimestampConsumedPrevious_s;
  input Real gpsTimestampConsumedPrevious_s;
  output State working;
  output Boolean mocapCorrectionAccepted;
  output Boolean gpsPositionCorrectionAccepted;
  output Boolean gpsVelocityCorrectionAccepted;
  output Boolean correctionAttempted;
  output Boolean correctionAccepted;
  output Integer correctionOutcome;
  output Integer correctionSource;
  output Real normalizedInnovationSquared;
  output Real mocapTimestampConsumedNext_s;
  output Real gpsTimestampConsumedNext_s;
algorithm
  working := copyState(previous);
  mocapCorrectionAccepted := false;
  gpsPositionCorrectionAccepted := false;
  gpsVelocityCorrectionAccepted := false;
  correctionAttempted := false;
  correctionAccepted := false;
  correctionOutcome := CorrectionNotAttempted;
  correctionSource := SourceNone;
  normalizedInnovationSquared := 0.0;
  mocapTimestampConsumedNext_s := mocapTimestampConsumedPrevious_s;
  gpsTimestampConsumedNext_s := gpsTimestampConsumedPrevious_s;
  if reseedNow then
    // A re-seed happened this tick: the state is already on the anchor
    // sample with the restored prior, so no correction is attempted.
    correctionAttempted := false;
  elseif mocapNew
      and (not anchorExclusive or anchorPresent == SourceMocap) then
    (working, mocapCorrectionAccepted, correctionOutcome,
     normalizedInnovationSquared) :=
      correctMocap(working, mocap, tuning.innovationGate,
        imuTimestampHeldNext_s - mocap.timestamp_s,
        imuAngularVelocityHeldNext_rad_s,
        imuSpecificForceHeldNext_m_s2, gravityWorldEnu_m_s2,
        tuning.maximumAidingDelay_s,
        tuning.useSemiDirectBias);
    correctionAttempted := true;
    correctionAccepted := mocapCorrectionAccepted;
    correctionSource := SourceMocap;
    mocapTimestampConsumedNext_s := mocap.timestamp_s;
  elseif gpsNew and gps.positionValid
      and gps.velocityValid
      and (not anchorExclusive or anchorPresent == SourceGps) then
    (working, gpsPositionCorrectionAccepted, correctionOutcome,
     normalizedInnovationSquared) :=
      correctGps(working, gps, tuning.innovationGate,
        imuTimestampHeldNext_s - gps.timestamp_s,
        imuAngularVelocityHeldNext_rad_s,
        imuSpecificForceHeldNext_m_s2, gravityWorldEnu_m_s2,
        tuning.maximumAidingDelay_s,
        if predictionAccepted and imu.integrationTime_s > 1.0e-6 then
          cat(1,
            cat(2, tuning.processNoise.gyroscope_rad2_s, zeros(3, 3)),
            cat(2, zeros(3, 3), tuning.processNoise.accelerometer_m2_s3))
            / imu.integrationTime_s
        else zeros(6, 6),
        if predictionAccepted and imu.integrationTime_s > 1.0e-6 then
          imu.integrationTime_s else 0.0,
        tuning.useSemiDirectBias);
    gpsVelocityCorrectionAccepted := gpsPositionCorrectionAccepted;
    correctionAttempted := true;
    correctionAccepted := gpsPositionCorrectionAccepted;
    correctionSource := SourceGps;
    gpsTimestampConsumedNext_s := gps.timestamp_s;
  elseif gpsNew and gps.positionValid
      and (not anchorExclusive or anchorPresent == SourceGps) then
    (working, gpsPositionCorrectionAccepted, correctionOutcome,
     normalizedInnovationSquared) :=
      correctGpsPosition(working, gps, tuning.innovationGate,
        imuTimestampHeldNext_s - gps.timestamp_s,
        imuAngularVelocityHeldNext_rad_s,
        imuSpecificForceHeldNext_m_s2, gravityWorldEnu_m_s2,
        tuning.maximumAidingDelay_s,
        tuning.useSemiDirectBias);
    correctionAttempted := true;
    correctionAccepted := gpsPositionCorrectionAccepted;
    correctionSource := SourceGps;
    gpsTimestampConsumedNext_s := gps.timestamp_s;
  elseif gpsNew and gps.velocityValid
      and (not anchorExclusive or anchorPresent == SourceGps) then
    (working, gpsVelocityCorrectionAccepted, correctionOutcome,
     normalizedInnovationSquared) :=
      correctGpsVelocity(working, gps, tuning.innovationGate,
        imuTimestampHeldNext_s - gps.timestamp_s,
        imuAngularVelocityHeldNext_rad_s,
        imuSpecificForceHeldNext_m_s2, gravityWorldEnu_m_s2,
        tuning.maximumAidingDelay_s,
        tuning.useSemiDirectBias);
    correctionAttempted := true;
    correctionAccepted := gpsVelocityCorrectionAccepted;
    correctionSource := SourceGps;
    gpsTimestampConsumedNext_s := gps.timestamp_s;
  end if;
end correctAbsoluteAidingTick;