Estimation/StrapdownINS/ESKF/step.mo

Download original .moGitHub source

Documentation: Estimation.StrapdownINS.ESKF.step

within Estimation.StrapdownINS.ESKF;

function step
  "Predict, select absolute aiding, and fuse independent flow, height and heading"
  input Boolean initializedPrevious;
  input State previous;
  input Boolean reset;
  input Avionics.ImuSample imu;
  input Avionics.MocapSample mocap;
  input Avionics.GpsSample gps;
  input Avionics.MagnetometerSample magnetometer;
  input Avionics.BarometerSample barometer;
  input Avionics.OpticalFlowSample opticalFlow;
  input Real gravityWorldEnu_m_s2[3];
  input Real dt;
  input Tuning tuning;
  input Integer consecutiveRejectionsPrevious;
  input Real rejectionElapsedPrevious_s;
  input Integer mocapRejectionsPrevious;
  input Integer gpsRejectionsPrevious;
  input Integer opticalFlowRejectionsPrevious;
  input Integer anchorSourcePrevious
    "Latched anchor from the previous tick. Latched, not momentary: a
     source going quiet between its own samples must not read as a change";
  input Real mocapStalePrevious_s;
  input Real gpsStalePrevious_s;
  input Real opticalFlowStalePrevious_s;
  input Real imuAngularVelocityHeldPrevious_rad_s[3];
  input Real imuSpecificForceHeldPrevious_m_s2[3];
  input Real imuTimestampHeldPrevious_s;
  input Real mocapTimestampConsumedPrevious_s;
  input Real gpsTimestampConsumedPrevious_s;
  input Real magnetometerTimestampConsumedPrevious_s;
  input Real barometerTimestampConsumedPrevious_s;
  input Real opticalFlowTimestampConsumedPrevious_s;
  input Real quietElapsedPrevious_s;
  input Real alignmentWaitPrevious_s;
  input Real pseudoPositionHoldPrevious_m[3];
  input Integer alignmentSourcePrevious;
  input Real alignmentSpecificForcePrevious_m_s2[3];
  input Real quietAngularRatePrevious_rad_s[3];
  input Boolean vehicleAtRest = false;
  output Real positionNext[3];
  output Real velocityNext[3];
  output Real quaternionNext[4];
  output Real gyroscopeBiasNext[3];
  output Real accelerometerBiasNext[3];
  output Covariance covarianceNext;
  output Boolean initializedNext;
  output Boolean predictionAccepted;
  output Boolean mocapCorrectionAccepted;
  output Boolean gpsPositionCorrectionAccepted;
  output Boolean gpsVelocityCorrectionAccepted;
  output Boolean magnetometerCorrectionAccepted;
  output Boolean barometerCorrectionAccepted;
  output Boolean opticalFlowCorrectionAccepted;
  output Integer consecutiveRejectionsNext;
  output Real rejectionElapsedNext_s
    "Wall-clock time the anchor source has held without moving the state.
     Advanced on estimator ticks, not on aiding attempts and not on the
     sensors` valid duty cycle. Cleared when the anchor accepts and when a
     genuinely different source takes over the anchor; FROZEN, not
     cleared, while no source is delivering";
  output Integer recoveryStage
    "Estimation.StrapdownINS.Recovery* code for the automatic
     recovery ladder driven by sustained rejection";
  output Integer correctionOutcome
    "Estimation.StrapdownINS.Correction* code for this tick's
     attempted aiding correction";
  output Integer correctionSource
    "Estimation.StrapdownINS.Source* code the outcome refers to";
  output Real normalizedInnovationSquared
    "NIS for this tick's attempted correction, or zero when none was attempted";
  output Boolean estimateValid
    "Whether the published navigation estimate may be consumed";
  output Boolean reseeded
    "True on the tick the automatic recovery ladder re-seeded the state
     from a fresh anchor sample after a sustained rejection window";
  output Integer mocapRejectionsNext
    "Consecutive rejections of the mocap source alone";
  output Integer gpsRejectionsNext
    "Consecutive rejections of the GPS source alone";
  output Integer opticalFlowRejectionsNext
    "Consecutive rejections of the optical-flow source alone";
  output Integer anchorSourceNext
    "Estimation.StrapdownINS.Source* code of the anchor in force";
  output Real mocapStaleNext_s;
  output Real gpsStaleNext_s;
  output Real opticalFlowStaleNext_s;
  output Real imuAngularVelocityHeldNext_rad_s[3]
    "IMU angular velocity to PUBLISH: the current sample when it is
     finite, otherwise the last finite one";
  output Real imuSpecificForceHeldNext_m_s2[3]
    "IMU specific force to PUBLISH, same hold rule";
  output Real imuTimestampHeldNext_s
    "IMU timestamp to PUBLISH, same hold rule";
  output Boolean imuPayloadHeld
    "True on a tick whose published IMU-derived outputs are held from an
     earlier sample because the current one was not finite";
  output Real mocapTimestampConsumedNext_s;
  output Real gpsTimestampConsumedNext_s;
  output Real magnetometerTimestampConsumedNext_s;
  output Real barometerTimestampConsumedNext_s;
  output Real opticalFlowTimestampConsumedNext_s;
  output Real alignmentWaitNext_s
    "Time with a usable IMU spent waiting for the alignment gate; zero once
     aligned";
  output Real quietElapsedNext_s
    "Unbroken time the IMU has reported a quasi-static vehicle: specific
     force within tolerance of gravity and angular rate below the limit";
  output Real pseudoPositionHoldNext_m[3]
    "Position the synthetic hold-position measurement is taken at: the
     current estimate while an anchor is live, frozen when it drops";
  output Integer alignmentSource
    "Estimation.StrapdownINS.Alignment* code of the alignment in force";
  output Real alignmentSpecificForce_m_s2[3]
    "Low-pass specific force the quasi-static test reads and the initial
     alignment levels on; the raw sample when no filter is configured";
  output Real quietAngularRateNext_rad_s[3]
    "Low-pass angular rate the quasi-static test reads";
  output Boolean pseudoPositionCorrectionAccepted;
  output Boolean zeroVelocityCorrectionAccepted;
  output Boolean stationaryImuCorrectionAccepted;
  output Covariance covarianceRootNext;
  output Real barometerBiasCrossCovarianceNext[TangentLength];
  output Real barometerBiasNext_m;
  output Real barometerBiasVarianceNext_m2;
protected
  State working;
  Boolean correctionAttempted;
  Boolean correctionAccepted;
  Boolean aidingLive;
  Boolean inflateCovarianceNow;
  Boolean aidingDivergent;
  Boolean reseedConfigured;
  Boolean reseedNow;
  Integer reseedSource;
  Boolean anchorExclusive;
  Boolean imuUsable;
  Boolean imuPayloadFinite;
  Boolean ladderConfigured;
  Integer anchorPresent;
  Boolean mocapSeedUsable;
  Boolean gpsSeedUsable;
  Boolean magnetometerSeedUsable;
  Boolean mocapNew;
  Boolean gpsNew;
  Boolean magnetometerNew;
  Boolean barometerNew;
  Boolean opticalFlowNew;
  Real mocapSeedQuaternionNorm;
  Real specificForceMagnitude;
  Real angularRateMagnitude;
  Real gravityMagnitude;
  Real sampleSpecificForce_m_s2[3];
  Real sampleAngularRate_rad_s[3];
  Real quietFilterGain;
  Boolean quietFilterSeeded;
  Boolean imuQuiet;
  Boolean alignmentGateConfigured;
  Boolean alignmentReady;
  Boolean alignmentTimedOut;
  Boolean alignmentPending;
algorithm
  predictionAccepted := false;
  mocapCorrectionAccepted := false;
  gpsPositionCorrectionAccepted := false;
  gpsVelocityCorrectionAccepted := false;
  magnetometerCorrectionAccepted := false;
  barometerCorrectionAccepted := false;
  opticalFlowCorrectionAccepted := false;
  correctionAttempted := false;
  correctionAccepted := false;
  correctionOutcome := CorrectionNotAttempted;
  correctionSource := SourceNone;
  normalizedInnovationSquared := 0.0;
  reseeded := false;
  pseudoPositionCorrectionAccepted := false;
  zeroVelocityCorrectionAccepted := false;
  stationaryImuCorrectionAccepted := false;
  alignmentSource := alignmentSourcePrevious;
  alignmentSpecificForce_m_s2 := alignmentSpecificForcePrevious_m_s2;
  pseudoPositionHoldNext_m := pseudoPositionHoldPrevious_m;
  reseedNow := false;
  reseedSource := SourceNone;
  consecutiveRejectionsNext := 0;
  rejectionElapsedNext_s := 0.0;
  // A commanded reset and first initialization clear every source's
  // history: the state they describe no longer exists.
  mocapRejectionsNext := 0;
  gpsRejectionsNext := 0;
  opticalFlowRejectionsNext := 0;
  mocapTimestampConsumedNext_s := mocapTimestampConsumedPrevious_s;
  gpsTimestampConsumedNext_s := gpsTimestampConsumedPrevious_s;
  magnetometerTimestampConsumedNext_s :=
    magnetometerTimestampConsumedPrevious_s;
  barometerTimestampConsumedNext_s := barometerTimestampConsumedPrevious_s;
  opticalFlowTimestampConsumedNext_s :=
    opticalFlowTimestampConsumedPrevious_s;

  imuPayloadFinite := imu.valid and imuSampleFinite(imu);
  // Cross-clock delivery is level-triggered: the producer holds the most
  // recent packet, and timestamp novelty makes consumption exactly once.
  imuUsable := imuPayloadFinite
    and imu.timestamp_s > imuTimestampHeldPrevious_s + 1.0e-9;
  sampleSpecificForce_m_s2 := if imu.integrationTime_s > 1.0e-6
    then imu.deltaVelocityBodyFlu_m_s / imu.integrationTime_s
    else imu.specificForceBodyFlu_m_s2;
  sampleAngularRate_rad_s := if imu.integrationTime_s > 1.0e-6
    then imu.deltaAngleBodyFlu_rad / imu.integrationTime_s
    else imu.angularVelocityBodyFlu_rad_s;
  quietFilterGain := if tuning.quietFilterTimeConstant_s > 0.0
      and tuning.quietFilterTimeConstant_s < FiniteMagnitudeLimit
    then min(1.0, dt / tuning.quietFilterTimeConstant_s) else 1.0;
  quietFilterSeeded := alignmentSpecificForcePrevious_m_s2
    * alignmentSpecificForcePrevious_m_s2 > 0.0;
  if not imuUsable then
    alignmentSpecificForce_m_s2 := alignmentSpecificForcePrevious_m_s2;
    quietAngularRateNext_rad_s := quietAngularRatePrevious_rad_s;
  elseif not quietFilterSeeded then
    alignmentSpecificForce_m_s2 := sampleSpecificForce_m_s2;
    quietAngularRateNext_rad_s := sampleAngularRate_rad_s;
  else
    alignmentSpecificForce_m_s2 := alignmentSpecificForcePrevious_m_s2
      + quietFilterGain
        * (sampleSpecificForce_m_s2 - alignmentSpecificForcePrevious_m_s2);
    quietAngularRateNext_rad_s := quietAngularRatePrevious_rad_s
      + quietFilterGain
        * (sampleAngularRate_rad_s - quietAngularRatePrevious_rad_s);
  end if;
  specificForceMagnitude := sqrt(alignmentSpecificForce_m_s2
    * alignmentSpecificForce_m_s2);
  angularRateMagnitude := sqrt(quietAngularRateNext_rad_s
    * quietAngularRateNext_rad_s);
  gravityMagnitude := sqrt(gravityWorldEnu_m_s2 * gravityWorldEnu_m_s2);
  imuQuiet := imuUsable
    and abs(specificForceMagnitude - gravityMagnitude)
      <= tuning.quietSpecificForceTolerance_m_s2
    and angularRateMagnitude <= tuning.quietAngularRateLimit_rad_s;
  // The window is a property of the inertial stream alone: a commanded
  // reset re-aligns from it but does not restart it, and a consumer that
  // holds reset asserted until the filter reports initialized must still
  // see the gate open. A tick that fails the test costs the window one
  // tick rather than all of it, so an isolated noise excursion delays the
  // gate instead of restarting it; sustained motion still drains it.
  quietElapsedNext_s := if not imuQuiet
    then max(0.0, quietElapsedPrevious_s - dt)
    else quietElapsedPrevious_s + dt;
  alignmentGateConfigured := tuning.initialAlignmentWindow_s > 0.0
    and tuning.initialAlignmentWindow_s < FiniteMagnitudeLimit;
  mocapNew := mocap.valid
    and abs(mocap.timestamp_s) < FiniteMagnitudeLimit
    and mocap.timestamp_s > mocapTimestampConsumedPrevious_s + 1.0e-9;
  gpsNew := gps.valid
    and abs(gps.timestamp_s) < FiniteMagnitudeLimit
    and gps.timestamp_s > gpsTimestampConsumedPrevious_s + 1.0e-9;
  magnetometerNew := magnetometer.valid
    and abs(magnetometer.timestamp_s) < FiniteMagnitudeLimit
    and magnetometer.timestamp_s
      > magnetometerTimestampConsumedPrevious_s + 1.0e-9;
  barometerNew := barometer.valid
    and abs(barometer.timestamp_s) < FiniteMagnitudeLimit
    and barometer.timestamp_s > barometerTimestampConsumedPrevious_s + 1.0e-9;
  opticalFlowNew := opticalFlow.valid
    and abs(opticalFlow.timestamp_s) < FiniteMagnitudeLimit
    and opticalFlow.timestamp_s
      > opticalFlowTimestampConsumedPrevious_s + 1.0e-9;

  // Initialization payloads must satisfy the same finite-data contract as
  // correction payloads before they can seed persistent state.
  mocapSeedUsable := mocap.valid;
  for axis in 1:3 loop
    mocapSeedUsable := mocapSeedUsable
      and abs(mocap.positionWorldEnu_m[axis]) < FiniteMagnitudeLimit;
  end for;
  for component in 1:4 loop
    mocapSeedUsable := mocapSeedUsable
      and abs(mocap.quaternionWorldBody[component]) < FiniteMagnitudeLimit;
  end for;
  // Normalizability, not just finiteness: a finite but vanishing
  // quaternion survives every magnitude check and then becomes a
  // full-scale garbage rotation when initialize() normalizes it.
  mocapSeedQuaternionNorm := mocap.quaternionWorldBody
    * mocap.quaternionWorldBody;
  mocapSeedUsable := mocapSeedUsable
    and sqrt(mocapSeedQuaternionNorm) >= MinimumSeedQuaternionNorm;
  gpsSeedUsable := gps.valid and gps.positionValid;
  for axis in 1:3 loop
    gpsSeedUsable := gpsSeedUsable
      and abs(gps.positionWorldEnu_m[axis]) < FiniteMagnitudeLimit;
  end for;
  magnetometerSeedUsable := magnetometer.valid;
  for axis in 1:3 loop
    magnetometerSeedUsable := magnetometerSeedUsable
      and abs(magnetometer.magneticFieldBodyFlu_T[axis]) < FiniteMagnitudeLimit
      and abs(tuning.localMagneticFieldWorldEnu_T[axis]) < FiniteMagnitudeLimit
      and magnetometer.covarianceBody_T2[axis, axis] > 0.0;
  end for;

  imuPayloadHeld := not imuPayloadFinite;
  if imuPayloadFinite then
    imuAngularVelocityHeldNext_rad_s := imu.angularVelocityBodyFlu_rad_s;
    imuSpecificForceHeldNext_m_s2 := imu.specificForceBodyFlu_m_s2;
    imuTimestampHeldNext_s := imu.timestamp_s;
  else
    imuAngularVelocityHeldNext_rad_s := imuAngularVelocityHeldPrevious_rad_s;
    imuSpecificForceHeldNext_m_s2 := imuSpecificForceHeldPrevious_m_s2;
    imuTimestampHeldNext_s := imuTimestampHeldPrevious_s;
  end if;

  mocapStaleNext_s := if mocapNew then 0.0
    else mocapStalePrevious_s + dt;
  gpsStaleNext_s := if gpsNew
      and (gps.positionValid or gps.velocityValid) then 0.0
    else gpsStalePrevious_s + dt;
  opticalFlowStaleNext_s := if opticalFlowNew
    then 0.0 else opticalFlowStalePrevious_s + dt;

  // THE ANCHOR SOURCE: the most authoritative source still delivering.
  // The existing correction priority already ranks the sources by
  // authority -- mocap is external ground truth, GPS is an absolute
  // position fix, optical flow is a relative velocity inference that
  // cannot observe position at all -- so the same order names the source
  // the filter is supposed to be anchored to.
  anchorPresent := if mocapStaleNext_s <= tuning.aidingStaleTimeout_s then
      SourceMocap
    elseif gpsStaleNext_s <= tuning.aidingStaleTimeout_s then SourceGps
    elseif opticalFlowStaleNext_s <= tuning.aidingStaleTimeout_s then
      SourceOpticalFlow
    else SourceNone;

  anchorSourceNext := if anchorPresent <> SourceNone then anchorPresent
    else anchorSourcePrevious;
  aidingLive := anchorPresent <> SourceNone;

  ladderConfigured := tuning.covarianceInflateWindow_s > 0.0
    and tuning.covarianceInflateWindow_s < FiniteMagnitudeLimit
    and tuning.covarianceInflateTimeConstant_s > 0.0
    and tuning.covarianceInflateTimeConstant_s < FiniteMagnitudeLimit
    and tuning.aidingStaleTimeout_s > 0.0
    and tuning.aidingStaleTimeout_s < FiniteMagnitudeLimit
    and tuning.aidingDivergentWindow_s > tuning.covarianceInflateWindow_s
    and tuning.aidingDivergentWindow_s < FiniteMagnitudeLimit;

  inflateCovarianceNow := ladderConfigured and initializedPrevious
    and not reset
    and rejectionElapsedPrevious_s >= tuning.covarianceInflateWindow_s;
  aidingDivergent := ladderConfigured and initializedPrevious and not reset
    and rejectionElapsedPrevious_s >= tuning.aidingDivergentWindow_s;
  // STAGE 3 configuration admission. Re-seed is opt-in: a non-positive or
  // non-finite window disables it, and an enabled window that does not
  // exceed the divergent window disables it too. A misconfigured value
  // therefore silences only the re-seed and never disturbs the two
  // always-on stages, the same affirmative-admission discipline the rest of
  // the ladder uses on its own windows.
  reseedConfigured := ladderConfigured
    and tuning.aidingReseedWindow_s > 0.0
    and tuning.aidingReseedWindow_s < FiniteMagnitudeLimit
    and tuning.aidingReseedWindow_s > tuning.aidingDivergentWindow_s;
  // The re-seed fires only past the re-seed window, on the anchor source
  // PRESENT this tick, and only when that source delivered a fresh sample
  // this tick that passes the same finite-data seed contract the
  // initialization branch enforces (mocap position and a normalizable
  // attitude; GPS position with positionValid). A non-finite payload falls
  // through exactly as an absent one, so a NaN can never seed the state.
  reseedNow := reseedConfigured and initializedPrevious and not reset
    and rejectionElapsedPrevious_s >= tuning.aidingReseedWindow_s
    and ((anchorPresent == SourceMocap and mocapNew and mocapSeedUsable)
      or (anchorPresent == SourceGps and gpsNew and gpsSeedUsable));
  recoveryStage := if not ladderConfigured then RecoveryMisconfigured
    elseif aidingDivergent then RecoveryAidingDivergent
    elseif inflateCovarianceNow then RecoveryCovarianceInflated
    else RecoveryNominal;
  // Exclusivity keys to the source PRESENT this tick, not the latched one:
  // while the anchor is between samples there is nothing for it to be
  // exclusive against, and blocking the others would simply stop all
  // correction during re-acquisition.
  anchorExclusive := recoveryStage == RecoveryCovarianceInflated
    and anchorPresent <> SourceNone;


  alignmentWaitNext_s := if initializedPrevious and not reset then 0.0
    elseif imuUsable then alignmentWaitPrevious_s + dt
    else alignmentWaitPrevious_s;
  alignmentTimedOut := tuning.initialAlignmentTimeout_s > 0.0
    and tuning.initialAlignmentTimeout_s < FiniteMagnitudeLimit
    and alignmentWaitNext_s >= tuning.initialAlignmentTimeout_s;
  alignmentReady := mocapSeedUsable or not alignmentGateConfigured
    or (imuUsable
      and (quietElapsedNext_s >= tuning.initialAlignmentWindow_s
        or alignmentTimedOut));
  alignmentPending := (not initializedPrevious or reset)
    and not alignmentReady;
  if alignmentPending then
    working := copyState(previous);
    alignmentSource := AlignmentNone;
  elseif not initializedPrevious or reset then
    (working, pseudoPositionHoldNext_m, alignmentSource,
     magnetometerCorrectionAccepted, magnetometerTimestampConsumedNext_s) :=
      initializeTick(mocap, gps, magnetometer, tuning,
        mocapSeedUsable, gpsSeedUsable, magnetometerSeedUsable, imuUsable,
        alignmentGateConfigured, imuQuiet, alignmentTimedOut,
        alignmentSpecificForce_m_s2, quietFilterGain, alignmentWaitNext_s,
        dt, gravityWorldEnu_m_s2, imuTimestampHeldNext_s,
        imuAngularVelocityHeldNext_rad_s, imuSpecificForceHeldNext_m_s2,
        magnetometerTimestampConsumedPrevious_s);
    // A seed is already an observation of the position. Consume that packet
    // here so the same held noise is not fused again on the next IMU tick.
    if mocapSeedUsable then
      mocapTimestampConsumedNext_s := if abs(mocap.timestamp_s) < FiniteMagnitudeLimit
        then mocap.timestamp_s else mocapTimestampConsumedPrevious_s;
    elseif gpsSeedUsable then
      gpsTimestampConsumedNext_s := if abs(gps.timestamp_s) < FiniteMagnitudeLimit
        then gps.timestamp_s else gpsTimestampConsumedPrevious_s;
    end if;
  else
    (
      working, predictionAccepted, stationaryImuCorrectionAccepted,
      mocapCorrectionAccepted, gpsPositionCorrectionAccepted, gpsVelocityCorrectionAccepted,
      magnetometerCorrectionAccepted, barometerCorrectionAccepted, opticalFlowCorrectionAccepted,
      correctionAttempted, correctionAccepted, correctionOutcome,
      correctionSource, normalizedInnovationSquared, reseeded,
      reseedSource, zeroVelocityCorrectionAccepted, pseudoPositionCorrectionAccepted,
      pseudoPositionHoldNext_m, mocapTimestampConsumedNext_s, gpsTimestampConsumedNext_s,
      magnetometerTimestampConsumedNext_s, barometerTimestampConsumedNext_s, opticalFlowTimestampConsumedNext_s
    ) := advanceStateTick(
      previous, imu, mocap,
      gps, magnetometer, barometer,
      opticalFlow, tuning, gravityWorldEnu_m_s2,
      dt, inflateCovarianceNow, imuUsable,
      imuPayloadFinite, vehicleAtRest, reseedNow,
      anchorExclusive, mocapNew, gpsNew,
      magnetometerNew, barometerNew, opticalFlowNew,
      aidingLive, ladderConfigured, imuQuiet,
      anchorPresent, rejectionElapsedPrevious_s, imuTimestampHeldNext_s,
      imuAngularVelocityHeldNext_rad_s, imuSpecificForceHeldNext_m_s2, pseudoPositionHoldPrevious_m,
      mocapTimestampConsumedPrevious_s, gpsTimestampConsumedPrevious_s, magnetometerTimestampConsumedPrevious_s,
      barometerTimestampConsumedPrevious_s, opticalFlowTimestampConsumedPrevious_s
    );

    if anchorPresent == SourceNone then
      // Total aiding loss FREEZES the clock; it does not clear it.
      // Clearing here was wrong in a way that reads as correct: losing
      // every source is not evidence that a divergence has been resolved,
      // it is a strictly worse situation, and de-escalating the report to
      // nominal at that moment tells the wrapper the opposite of the
      // truth. Measured: a divergent stage fell back to nominal 403 ms
      // after total dropout, while completely unaided. Frozen, the report
      // persists and resumes from where it stood if aiding returns.
      consecutiveRejectionsNext := consecutiveRejectionsPrevious;
      rejectionElapsedNext_s := rejectionElapsedPrevious_s;
    elseif anchorSourceNext <> anchorSourcePrevious then
      // A different live source has taken over. Its elapsed time describes
      // the source that just lost the anchor, so it does not apply.
      consecutiveRejectionsNext := 0;
      rejectionElapsedNext_s := 0.0;
    elseif correctionAttempted and correctionSource == anchorPresent then
      if correctionAccepted then
        consecutiveRejectionsNext := 0;
        rejectionElapsedNext_s := 0.0;
      else
        consecutiveRejectionsNext := consecutiveRejectionsPrevious + 1;
        rejectionElapsedNext_s := rejectionElapsedPrevious_s + dt;
      end if;
    else
      // The anchor held but said nothing this tick, whether because it is
      // between samples or because it has gone quiet. Either way the
      // state has spent another tick unanchored, so the clock advances.
      consecutiveRejectionsNext := consecutiveRejectionsPrevious;
      rejectionElapsedNext_s := rejectionElapsedPrevious_s + dt;
    end if;

    mocapRejectionsNext := if correctionSource == SourceMocap then
        (if correctionAccepted then 0 else mocapRejectionsPrevious + 1)
      else mocapRejectionsPrevious;
    gpsRejectionsNext := if correctionSource == SourceGps then
        (if correctionAccepted then 0 else gpsRejectionsPrevious + 1)
      else gpsRejectionsPrevious;
    opticalFlowRejectionsNext :=
      if not reseedNow and opticalFlowNew
          and (not anchorExclusive or anchorPresent == SourceOpticalFlow) then
        (if opticalFlowCorrectionAccepted then 0
          else opticalFlowRejectionsPrevious + 1)
      else opticalFlowRejectionsPrevious;

    // STAGE 3 telemetry and clock reset. The re-seed is a deliberate
    // replacement of the state, so the tick reports CorrectionReseeded
    // rather than the gate acceptance the same-tick fusion recorded, and it
    // clears the rejection clock and every per-source counter: the
    // divergence they were measuring has been resolved by construction.
    if reseeded then
      correctionOutcome := CorrectionReseeded;
      correctionSource := reseedSource;
      consecutiveRejectionsNext := 0;
      rejectionElapsedNext_s := 0.0;
      mocapRejectionsNext := 0;
      gpsRejectionsNext := 0;
      opticalFlowRejectionsNext := 0;
    end if;
  end if;

  positionNext := working.positionWorldEnu_m;
  velocityNext := working.velocityWorldEnu_m_s;
  quaternionNext := working.quaternionWorldBody;
  gyroscopeBiasNext := working.gyroscopeBiasBodyFlu_rad_s;
  accelerometerBiasNext := working.accelerometerBiasBodyFlu_m_s2;
  covarianceNext := working.covariance;
  covarianceRootNext := working.covarianceRoot;
  barometerBiasCrossCovarianceNext := working.barometerBiasCrossCovariance;
  barometerBiasNext_m := working.barometerBias_m;
  barometerBiasVarianceNext_m2 := working.barometerBiasVariance_m2;
  initializedNext := not alignmentPending;
  estimateValid := initializedNext
    and nominalStateFinite(positionNext, velocityNext, quaternionNext);
end step;