Tests.EstimatorHealthTests.run

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

function run
    output Boolean passed;
  protected
    constant Real tolerance = 1.0e-6;
    constant Real dt = 0.01;
    // Three rejected ticks of aiding at dt reach the inflate window.
    constant Real inflateWindow = 3.0 * dt;
    constant Real divergentWindow = 8.0 * dt;
    constant Real gate = 6.0;
    constant Real farAway[3] = {1.0e3, 0.0, 0.0};
    // Close enough to be admissible at the covariance one ramp step
    // produces, so tick 6 exercises acceptance through the ordinary
    // correction path while the ladder is active. It is NOT sized to the
    // mission envelope: the ramp reaches the envelope over seconds, not
    // in the single tick this unrolled fixture can express.
    constant Real admissibleFix[3] = {2.0, 0.0, 0.0};
    Real marginal[2, 2];
    Real solution[2, 1];
    Boolean okDefault;
    Boolean okTightened;
    Estimation.StrapdownINS.ESKF.VarianceLimits limits;
    Estimation.StrapdownINS.ESKF.Covariance inflated;
    Estimation.StrapdownINS.ESKF.Covariance limited;
    Estimation.StrapdownINS.InitialVariances initialVariances;
    Estimation.StrapdownINS.ESKF.State prior;
    Estimation.StrapdownINS.ESKF.State gatedState;
    Estimation.StrapdownINS.ESKF.State ungatedState;
    Avionics.GpsSample gpsFar;
    Boolean gatedAccepted;
    Boolean ungatedAccepted;
    Real position[3];
    Real velocity[3];
    Real quaternion[4];
    Real gyroscopeBias[3];
    Real accelerometerBias[3];
    Estimation.StrapdownINS.ESKF.Covariance covariance;
    Boolean initialized;
    Real positionNext[3];
    Real velocityNext[3];
    Real quaternionNext[4];
    Real gyroscopeBiasNext[3];
    Real accelerometerBiasNext[3];
    Estimation.StrapdownINS.ESKF.Covariance covarianceNext;
    Boolean initializedNext;
    Integer rejectionsNext;
    Integer rejections;
    Real rejectionElapsedNext_s;
    Real rejectionElapsed;
    Boolean mocapOk;
    Integer recoveryStage;
    Integer correctionOutcome;
    Boolean estimateValid;
    Boolean boundsOk;
    Boolean startupOk;
    Boolean rejectionOk;
    Boolean inflateOk;
    Boolean recoveryOk;
    Integer gatedReason;
    Integer anchorPrev;
    Real varianceBeforeInflate;
    Real varianceBeforeAccept;
    Integer ungatedReason;
    Real gatedNis;
    Real ungatedNis;
    // Stage 3 re-seed fixture. A GPS stream jumps 2 km and is gate-rejected
    // every tick, so the anchor clock runs unbroken through stages 1 and 2
    // and into the re-seed window. With the window enabled the state must be
    // re-seeded onto the fix with the initial variances restored; with it
    // disabled (the default) the same sequence must never re-seed.
    constant Real reseedInflate = 1.0 * dt;
    constant Real reseedDivergent = 2.0 * dt;
    constant Real reseedWindow = 3.0 * dt;
    constant Real farGps[3] = {2.0e3, 0.0, 0.0};
    constant Real farGpsVelocity[3] = {1.0, 0.0, 0.0};
    Boolean gpsOk;
    Boolean reseededOut;
    Boolean reseedFireOk;
    Boolean reseedDisabledOk;
  algorithm
    // Fix 1: the pivot threshold scales with the working precision. A
    // trailing pivot of 1e-8 is comfortably above the binary64 floor
    // (n * eps * scale ~ 4.4e-16), so the default accepts it; an
    // explicit relativeTolerance of 1e-6 must reject the same matrix.
    marginal := [1.0, 1.0; 1.0, 1.0 + 1.0e-8];
    (solution, okDefault) := LinearAlgebra.solveSPD(marginal, [1.0; 0.0]);
    (solution, okTightened) := LinearAlgebra.solveSPD(
      marginal, [1.0; 0.0], 1.0e-6);
    assert(okDefault,
      "Precision-scaled default rejected a representable trailing pivot");
    assert(not okTightened,
      "Explicit relativeTolerance floor did not tighten the pivot check");

    // Fix 2: diagonal variance limiting preserves symmetry and
    // correlation coefficients while capping out-of-envelope growth.
    limits := 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));
    inflated := identity(15) * 0.5;
    inflated[1, 1] := 4.0e4;
    inflated[4, 4] := 1.6e3;
    inflated[1, 4] := 2.0e3;
    inflated[4, 1] := 2.0e3;
    limited := Estimation.StrapdownINS.ESKF.limitCovariance(
      inflated, limits);
    assert(abs(limited[1, 1] - 1.0e4) < tolerance and
      abs(limited[4, 4] - 4.0e2) < tolerance,
      "Variance limiting did not clamp the inflated diagonal");
    assert(abs(limited[1, 4] - 5.0e2) < tolerance and
      abs(limited[4, 1] - limited[1, 4]) < tolerance,
      "Variance limiting broke symmetry or the preserved correlation");
    assert(abs(limited[2, 2] - 0.5) < tolerance,
      "Variance limiting rescaled an in-envelope variance");

    // A non-finite diagonal entry must be CONTAINED, not merely detected.
    // The two-sided scaling cannot do it on its own: the rescale factor for
    // such a row is 0.0, and 0.0 times an overflowed entry is not a zero the
    // matrix can carry -- for a true infinity it is NaN, and NaN in one
    // position of a covariance ends every later Cholesky and every later
    // gain. The row and column are therefore overwritten rather than scaled,
    // and the diagonal is rebuilt at its bound so the state reads maximally
    // uncertain instead of perfectly known.
    inflated := identity(15) * 0.5;
    inflated[1, 1] := 1.0e300;
    inflated[1, 4] := 1.0e150;
    inflated[4, 1] := 1.0e150;
    inflated[4, 4] := 1.0e2;
    limited := Estimation.StrapdownINS.ESKF.limitCovariance(
      inflated, limits);
    assert(abs(limited[1, 1] - 1.0e4) < tolerance,
      "A non-finite variance was not rebuilt at its envelope bound");
    assert(limited[1, 4] == 0.0 and limited[4, 1] == 0.0,
      "A rejected covariance row leaked through its off-diagonal entries");
    assert(abs(limited[4, 4] - 1.0e2) < tolerance,
      "Containing a rejected row disturbed an in-envelope variance");
    assert(abs(limited[2, 2] - 0.5) < tolerance,
      "Containing a rejected row disturbed an unrelated variance");

    // Fix 4: chi-square innovation gate. A 100 m residual against a
    // ~1 m2 innovation variance has NIS ~ 1e4, far beyond 6 per degree
    // of freedom, so the gated call must reject without touching the
    // state while the ungated call still accepts.
    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));
    prior := Estimation.StrapdownINS.ESKF.initialize(
      zeros(3), {1.0, 0.0, 0.0, 0.0}, initialVariances);
    gpsFar := Avionics.GpsSample(
      valid=true,
      fresh=true,
      positionValid=true,
      velocityValid=false,
      timestamp_s=dt,
      geodetic_deg_m=zeros(3),
      positionWorldEnu_m={100.0, 0.0, 0.0},
      velocityWorldEnu_m_s=zeros(3),
      positionCovarianceWorld_m2=identity(3) * 0.01,
      velocityCovarianceWorld_m2_s2=identity(3) * 0.01);
    (gatedState, gatedAccepted, gatedReason, gatedNis) :=
      Estimation.StrapdownINS.ESKF.correctGpsPosition(
        prior, gpsFar, gate);
    (ungatedState, ungatedAccepted, ungatedReason, ungatedNis) :=
      Estimation.StrapdownINS.ESKF.correctGpsPosition(prior, gpsFar);
    assert(not gatedAccepted and
      gatedReason == Estimation.StrapdownINS.CorrectionRejectedGate
      and gatedNis > gate * 3.0,
      "The innovation gate did not reject a grossly inconsistent residual");
    assert(Tests.Assertions.maxAbsVector(
        gatedState.positionWorldEnu_m - prior.positionWorldEnu_m)
        < tolerance and
      Tests.Assertions.maxAbsMatrix(
        gatedState.covariance - prior.covariance) < tolerance,
      "A gate-rejected correction modified the state");
    assert(ungatedAccepted and
      ungatedReason == Estimation.StrapdownINS.CorrectionAccepted,
      "Disabling the innovation gate did not restore acceptance");

    // Fix 3: persistent rejection drives the two-stage recovery ladder,
    // which NEVER re-seeds the state from the stream it is rejecting.
    // Tick 1 initializes from mocap at the origin; ticks 2-4 feed a mocap
    // position 1 km away that the gate rejects, advancing the rejection
    // clock; tick 5 crosses the inflate window, so the covariance is
    // ramped by one bounded step with the state left exactly where it was;
    // tick 6 offers a fix consistent with that ramped covariance, which is
    // admitted through the ordinary correction path and pulls the estimate
    // over continuously.
    //
    // The 1 km fix is never adopted at any point. That is the property
    // this test exists to pin: the previous implementation re-ran the
    // declared initialization policy here and seeded position straight
    // from the rejected 1 km sample while zeroing velocity and resetting
    // attitude to the identity quaternion, all with estimate.valid still
    // true.
    //
    // The six ticks are written out rather than driven by a `for` loop.
    // A loop body has to hand each tick's outputs back to the next tick
    // through `state := stateNext` copies, and rumoca 0.9.20 drops such a
    // copy across the loop back-edge in both the folded-parameter and the
    // compiled simulation paths: every iteration then re-reads the
    // pre-loop value, the estimator re-initializes on every tick, and the
    // health assertions below fail against a correct model. OpenModelica
    // evaluates the loop form correctly. Straight-line copies are
    // compiled correctly, so the unrolled form below tests exactly the
    // intended sequence.
    position := zeros(3);
    velocity := zeros(3);
    quaternion := {1.0, 0.0, 0.0, 0.0};
    gyroscopeBias := zeros(3);
    accelerometerBias := zeros(3);
    covariance := zeros(15, 15);
    initialized := false;
    rejections := 0;
    rejectionElapsed := 0.0;
    anchorPrev := 0;
    boundsOk := true;

    // Tick 1: first sample initializes from mocap at the origin.
    (positionNext, velocityNext, quaternionNext, gyroscopeBiasNext,
     accelerometerBiasNext, covarianceNext, initializedNext, rejectionsNext,
     rejectionElapsedNext_s, mocapOk, recoveryStage, correctionOutcome,
     estimateValid) := mocapTick(
      initialized, position, velocity, quaternion, gyroscopeBias,
      accelerometerBias, covariance, rejections, rejectionElapsed, zeros(3),
      dt, dt, gate, inflateWindow, divergentWindow, 0.5, 0.5, anchorPrev);
    position := positionNext;
    velocity := velocityNext;
    quaternion := quaternionNext;
    gyroscopeBias := gyroscopeBiasNext;
    accelerometerBias := accelerometerBiasNext;
    covariance := covarianceNext;
    initialized := initializedNext;
    rejections := rejectionsNext;
    rejectionElapsed := rejectionElapsedNext_s;
    anchorPrev := Estimation.StrapdownINS.SourceMocap;
    boundsOk := boundsOk and covariance[1, 1] <= 1.0e4 + tolerance;
    startupOk := initialized and rejections == 0 and estimateValid
      and recoveryStage == Estimation.StrapdownINS.RecoveryNominal;

    // Tick 2: the first 1 km mocap sample is gate-rejected; the clock
    // advances by one interval and the estimate must not move.
    (positionNext, velocityNext, quaternionNext, gyroscopeBiasNext,
     accelerometerBiasNext, covarianceNext, initializedNext, rejectionsNext,
     rejectionElapsedNext_s, mocapOk, recoveryStage, correctionOutcome,
     estimateValid) := mocapTick(
      initialized, position, velocity, quaternion, gyroscopeBias,
      accelerometerBias, covariance, rejections, rejectionElapsed, farAway,
      2.0 * dt, dt, gate, inflateWindow, divergentWindow, 0.5, 0.5, anchorPrev);
    position := positionNext;
    velocity := velocityNext;
    quaternion := quaternionNext;
    gyroscopeBias := gyroscopeBiasNext;
    accelerometerBias := accelerometerBiasNext;
    covariance := covarianceNext;
    initialized := initializedNext;
    rejections := rejectionsNext;
    rejectionElapsed := rejectionElapsedNext_s;
    anchorPrev := Estimation.StrapdownINS.SourceMocap;
    boundsOk := boundsOk and covariance[1, 1] <= 1.0e4 + tolerance;
    rejectionOk := not mocapOk and rejections == 1
      and correctionOutcome
        == Estimation.StrapdownINS.CorrectionRejectedGate
      and abs(rejectionElapsed - dt) < tolerance
      and abs(position[1]) < 1.0
      and recoveryStage == Estimation.StrapdownINS.RecoveryNominal;

    // Tick 3: clock reaches two intervals, still below the window.
    (positionNext, velocityNext, quaternionNext, gyroscopeBiasNext,
     accelerometerBiasNext, covarianceNext, initializedNext, rejectionsNext,
     rejectionElapsedNext_s, mocapOk, recoveryStage, correctionOutcome,
     estimateValid) := mocapTick(
      initialized, position, velocity, quaternion, gyroscopeBias,
      accelerometerBias, covariance, rejections, rejectionElapsed, farAway,
      3.0 * dt, dt, gate, inflateWindow, divergentWindow, 0.5, 0.5, anchorPrev);
    position := positionNext;
    velocity := velocityNext;
    quaternion := quaternionNext;
    gyroscopeBias := gyroscopeBiasNext;
    accelerometerBias := accelerometerBiasNext;
    covariance := covarianceNext;
    initialized := initializedNext;
    rejections := rejectionsNext;
    rejectionElapsed := rejectionElapsedNext_s;
    anchorPrev := Estimation.StrapdownINS.SourceMocap;
    boundsOk := boundsOk and covariance[1, 1] <= 1.0e4 + tolerance;
    rejectionOk := rejectionOk and not mocapOk and rejections == 2
      and correctionOutcome
        == Estimation.StrapdownINS.CorrectionRejectedGate
      and abs(rejectionElapsed - 2.0 * dt) < tolerance
      and abs(position[1]) < 1.0
      and recoveryStage == Estimation.StrapdownINS.RecoveryNominal;

    // Tick 4: the clock reaches the inflate window on this tick's OUTPUT,
    // so stage 1 is armed for the next tick but has not fired yet.
    (positionNext, velocityNext, quaternionNext, gyroscopeBiasNext,
     accelerometerBiasNext, covarianceNext, initializedNext, rejectionsNext,
     rejectionElapsedNext_s, mocapOk, recoveryStage, correctionOutcome,
     estimateValid) := mocapTick(
      initialized, position, velocity, quaternion, gyroscopeBias,
      accelerometerBias, covariance, rejections, rejectionElapsed, farAway,
      4.0 * dt, dt, gate, inflateWindow, divergentWindow, 0.5, 0.5, anchorPrev);
    position := positionNext;
    velocity := velocityNext;
    quaternion := quaternionNext;
    gyroscopeBias := gyroscopeBiasNext;
    accelerometerBias := accelerometerBiasNext;
    covariance := covarianceNext;
    initialized := initializedNext;
    rejections := rejectionsNext;
    rejectionElapsed := rejectionElapsedNext_s;
    anchorPrev := Estimation.StrapdownINS.SourceMocap;
    boundsOk := boundsOk and covariance[1, 1] <= 1.0e4 + tolerance;
    varianceBeforeInflate := covariance[1, 1];
    rejectionOk := rejectionOk and not mocapOk and rejections == 3
      and correctionOutcome
        == Estimation.StrapdownINS.CorrectionRejectedGate
      and abs(rejectionElapsed - inflateWindow) < tolerance
      and abs(position[1]) < 1.0
      and recoveryStage == Estimation.StrapdownINS.RecoveryNominal;

    // Tick 5: stage 1 fires. The position covariance RAMPS by one bounded
    // step and the state is UNTOUCHED. The 1 km sample is still rejected,
    // and nothing seeds position from it.
    //
    // The bound is the point. At dt = 0.01 against a 0.5 s time constant
    // one step multiplies a variance by exp(2*dt/tau) = 1.041, so the
    // assertion below pins a few percent of growth. The form this
    // replaced set the variance straight to the 1e4 envelope in this one
    // tick, which is what turned gating into adoption; an upper bound of
    // 2.0 fails loudly against that.
    (positionNext, velocityNext, quaternionNext, gyroscopeBiasNext,
     accelerometerBiasNext, covarianceNext, initializedNext, rejectionsNext,
     rejectionElapsedNext_s, mocapOk, recoveryStage, correctionOutcome,
     estimateValid) := mocapTick(
      initialized, position, velocity, quaternion, gyroscopeBias,
      accelerometerBias, covariance, rejections, rejectionElapsed, farAway,
      5.0 * dt, dt, gate, inflateWindow, divergentWindow, 0.5, 0.5, anchorPrev);
    position := positionNext;
    velocity := velocityNext;
    quaternion := quaternionNext;
    gyroscopeBias := gyroscopeBiasNext;
    accelerometerBias := accelerometerBiasNext;
    covariance := covarianceNext;
    initialized := initializedNext;
    rejections := rejectionsNext;
    rejectionElapsed := rejectionElapsedNext_s;
    anchorPrev := Estimation.StrapdownINS.SourceMocap;
    boundsOk := boundsOk and covariance[1, 1] <= 1.0e4 + tolerance;
    inflateOk := recoveryStage
        == Estimation.StrapdownINS.RecoveryCovarianceInflated
      and not mocapOk
      and abs(position[1]) < 1.0
      and abs(velocity[1]) < tolerance
      and abs(quaternion[1] - 1.0) < tolerance
      and covariance[1, 1] > varianceBeforeInflate
      and covariance[1, 1] < 2.0
      // Attitude is deliberately NOT inflated: a filter told its
      // orientation is unknown explains position residuals as rotation.
      and covariance[7, 7] < 1.0;
    varianceBeforeAccept := covariance[1, 1];

    // Tick 6: a fix close enough to be consistent with the ramped
    // covariance is accepted through the ORDINARY correction path, while
    // the ladder is still active, and pulls the estimate over. Recovery
    // happens by admitting a plausible measurement, never by seeding from
    // an implausible one -- and the active ladder does not block an
    // ordinary correction from the anchor.
    (positionNext, velocityNext, quaternionNext, gyroscopeBiasNext,
     accelerometerBiasNext, covarianceNext, initializedNext, rejectionsNext,
     rejectionElapsedNext_s, mocapOk, recoveryStage, correctionOutcome,
     estimateValid) := mocapTick(
      initialized, position, velocity, quaternion, gyroscopeBias,
      accelerometerBias, covariance, rejections, rejectionElapsed,
      admissibleFix, 6.0 * dt, dt, gate, inflateWindow, divergentWindow,
      0.5, 0.5, anchorPrev);
    position := positionNext;
    velocity := velocityNext;
    quaternion := quaternionNext;
    gyroscopeBias := gyroscopeBiasNext;
    accelerometerBias := accelerometerBiasNext;
    covariance := covarianceNext;
    initialized := initializedNext;
    rejections := rejectionsNext;
    rejectionElapsed := rejectionElapsedNext_s;
    anchorPrev := Estimation.StrapdownINS.SourceMocap;
    boundsOk := boundsOk and covariance[1, 1] <= 1.0e4 + tolerance;
    recoveryOk := mocapOk and rejections == 0
      and correctionOutcome
        == Estimation.StrapdownINS.CorrectionAccepted
      and abs(rejectionElapsed) < tolerance
      // Moved toward the fix without teleporting onto it. The bound is
      // deliberately loose at the low end: if the attitude trust region
      // clamps this correction the state advances in installments, which
      // is correct behaviour and must not fail the test.
      and position[1] > 0.1
      and position[1] < 2.5
      // An accepted correction must CONTRACT the position variance.
      // Stated against the pre-acceptance value rather than an absolute
      // bound, so it holds whether or not the trust region engaged the
      // partial-update covariance blend.
      and covariance[1, 1] < varianceBeforeAccept
      and estimateValid;

    // ---- Stage 3 re-seed ENABLED: the ladder re-seeds onto a rejected but
    // persistent GPS stream once the divergence outlasts the re-seed window.
    position := zeros(3);
    velocity := zeros(3);
    quaternion := {1.0, 0.0, 0.0, 0.0};
    gyroscopeBias := zeros(3);
    accelerometerBias := zeros(3);
    covariance := zeros(15, 15);
    initialized := false;
    rejections := 0;
    rejectionElapsed := 0.0;
    anchorPrev := 0;

    // Tick 1: initialize from GPS at the origin.
    (positionNext, velocityNext, quaternionNext, gyroscopeBiasNext,
     accelerometerBiasNext, covarianceNext, initializedNext, rejectionsNext,
     rejectionElapsedNext_s, gpsOk, recoveryStage, correctionOutcome,
     estimateValid, reseededOut) := gpsTick(
      initialized, position, velocity, quaternion, gyroscopeBias,
      accelerometerBias, covariance, rejections, rejectionElapsed,
      zeros(3), zeros(3), true, true, dt, dt, gate, reseedInflate,
      reseedDivergent, reseedWindow, anchorPrev);
    position := positionNext;
    velocity := velocityNext;
    quaternion := quaternionNext;
    gyroscopeBias := gyroscopeBiasNext;
    accelerometerBias := accelerometerBiasNext;
    covariance := covarianceNext;
    initialized := initializedNext;
    rejections := rejectionsNext;
    rejectionElapsed := rejectionElapsedNext_s;
    anchorPrev := Estimation.StrapdownINS.SourceGps;

    // Ticks 2-4: the 2 km fix is gate-rejected while the clock runs through
    // stages 1 and 2. The estimate must stay at the origin the whole way.
    // Unrolled rather than looped: rumoca 0.9.20 drops the loop-carried
    // state copies across the back-edge, as the mocap ladder fixture above
    // documents.
    (positionNext, velocityNext, quaternionNext, gyroscopeBiasNext,
     accelerometerBiasNext, covarianceNext, initializedNext, rejectionsNext,
     rejectionElapsedNext_s, gpsOk, recoveryStage, correctionOutcome,
     estimateValid, reseededOut) := gpsTick(
      initialized, position, velocity, quaternion, gyroscopeBias,
      accelerometerBias, covariance, rejections, rejectionElapsed,
      farGps, farGpsVelocity, true, true, 2.0 * dt, dt, gate,
      reseedInflate, reseedDivergent, reseedWindow, anchorPrev);
    position := positionNext;
    velocity := velocityNext;
    quaternion := quaternionNext;
    gyroscopeBias := gyroscopeBiasNext;
    accelerometerBias := accelerometerBiasNext;
    covariance := covarianceNext;
    initialized := initializedNext;
    rejections := rejectionsNext;
    rejectionElapsed := rejectionElapsedNext_s;
    reseedFireOk := not reseededOut and abs(position[1]) < 1.0;

    (positionNext, velocityNext, quaternionNext, gyroscopeBiasNext,
     accelerometerBiasNext, covarianceNext, initializedNext, rejectionsNext,
     rejectionElapsedNext_s, gpsOk, recoveryStage, correctionOutcome,
     estimateValid, reseededOut) := gpsTick(
      initialized, position, velocity, quaternion, gyroscopeBias,
      accelerometerBias, covariance, rejections, rejectionElapsed,
      farGps, farGpsVelocity, true, true, 3.0 * dt, dt, gate,
      reseedInflate, reseedDivergent, reseedWindow, anchorPrev);
    position := positionNext;
    velocity := velocityNext;
    quaternion := quaternionNext;
    gyroscopeBias := gyroscopeBiasNext;
    accelerometerBias := accelerometerBiasNext;
    covariance := covarianceNext;
    initialized := initializedNext;
    rejections := rejectionsNext;
    rejectionElapsed := rejectionElapsedNext_s;
    reseedFireOk := reseedFireOk and not reseededOut and abs(position[1]) < 1.0;

    (positionNext, velocityNext, quaternionNext, gyroscopeBiasNext,
     accelerometerBiasNext, covarianceNext, initializedNext, rejectionsNext,
     rejectionElapsedNext_s, gpsOk, recoveryStage, correctionOutcome,
     estimateValid, reseededOut) := gpsTick(
      initialized, position, velocity, quaternion, gyroscopeBias,
      accelerometerBias, covariance, rejections, rejectionElapsed,
      farGps, farGpsVelocity, true, true, 4.0 * dt, dt, gate,
      reseedInflate, reseedDivergent, reseedWindow, anchorPrev);
    position := positionNext;
    velocity := velocityNext;
    quaternion := quaternionNext;
    gyroscopeBias := gyroscopeBiasNext;
    accelerometerBias := accelerometerBiasNext;
    covariance := covarianceNext;
    initialized := initializedNext;
    rejections := rejectionsNext;
    rejectionElapsed := rejectionElapsedNext_s;
    reseedFireOk := reseedFireOk and not reseededOut and abs(position[1]) < 1.0;

    // Tick 5: the clock has crossed the re-seed window, so this fresh fix
    // re-seeds the state onto itself with the initial variances restored.
    (positionNext, velocityNext, quaternionNext, gyroscopeBiasNext,
     accelerometerBiasNext, covarianceNext, initializedNext, rejectionsNext,
     rejectionElapsedNext_s, gpsOk, recoveryStage, correctionOutcome,
     estimateValid, reseededOut) := gpsTick(
      initialized, position, velocity, quaternion, gyroscopeBias,
      accelerometerBias, covariance, rejections, rejectionElapsed,
      farGps, farGpsVelocity, true, true, 5.0 * dt, dt, gate,
      reseedInflate, reseedDivergent, reseedWindow, anchorPrev);
    position := positionNext;
    velocity := velocityNext;
    covariance := covarianceNext;
    rejections := rejectionsNext;
    rejectionElapsed := rejectionElapsedNext_s;
    reseedFireOk := reseedFireOk
      and reseededOut
      and correctionOutcome == Estimation.StrapdownINS.CorrectionReseeded
      // Landed exactly on the fix: position from the sample, velocity from
      // the GPS velocity the sample reported.
      and abs(position[1] - farGps[1]) < tolerance
      and abs(velocity[1] - farGpsVelocity[1]) < tolerance
      // Position and velocity variance restored to the initial variances,
      // not left at the pre-seed gated value and not collapsed by a fusion.
      and abs(covariance[1, 1] - 1.0) < tolerance
      and abs(covariance[4, 4] - 1.0) < tolerance
      // Cross-covariance with the attitude block zeroed, attitude variance
      // kept rather than reset.
      and abs(covariance[1, 7]) < tolerance
      and abs(covariance[7, 1]) < tolerance
      and covariance[7, 7] > 0.0
      // The rejection clock and stage report are cleared by the re-seed.
      and rejections == 0
      and abs(rejectionElapsed) < tolerance
      and estimateValid;

    // ---- Stage 3 re-seed DISABLED (the default): the identical sequence
    // must never re-seed, and the persistent 2 km fix is never adopted.
    position := zeros(3);
    velocity := zeros(3);
    quaternion := {1.0, 0.0, 0.0, 0.0};
    gyroscopeBias := zeros(3);
    accelerometerBias := zeros(3);
    covariance := zeros(15, 15);
    initialized := false;
    rejections := 0;
    rejectionElapsed := 0.0;
    anchorPrev := 0;
    reseedDisabledOk := true;

    (positionNext, velocityNext, quaternionNext, gyroscopeBiasNext,
     accelerometerBiasNext, covarianceNext, initializedNext, rejectionsNext,
     rejectionElapsedNext_s, gpsOk, recoveryStage, correctionOutcome,
     estimateValid, reseededOut) := gpsTick(
      initialized, position, velocity, quaternion, gyroscopeBias,
      accelerometerBias, covariance, rejections, rejectionElapsed,
      zeros(3), zeros(3), true, true, dt, dt, gate, reseedInflate,
      reseedDivergent, 0.0, anchorPrev);
    position := positionNext;
    velocity := velocityNext;
    quaternion := quaternionNext;
    gyroscopeBias := gyroscopeBiasNext;
    accelerometerBias := accelerometerBiasNext;
    covariance := covarianceNext;
    initialized := initializedNext;
    rejections := rejectionsNext;
    rejectionElapsed := rejectionElapsedNext_s;
    anchorPrev := Estimation.StrapdownINS.SourceGps;

    // Ticks 2-6, unrolled: past the tick where the enabled window re-seeded,
    // the disabled default must still be rejecting the 2 km fix and holding
    // the estimate at the origin.
    (positionNext, velocityNext, quaternionNext, gyroscopeBiasNext,
     accelerometerBiasNext, covarianceNext, initializedNext, rejectionsNext,
     rejectionElapsedNext_s, gpsOk, recoveryStage, correctionOutcome,
     estimateValid, reseededOut) := gpsTick(
      initialized, position, velocity, quaternion, gyroscopeBias,
      accelerometerBias, covariance, rejections, rejectionElapsed,
      farGps, farGpsVelocity, true, true, 2.0 * dt, dt, gate,
      reseedInflate, reseedDivergent, 0.0, anchorPrev);
    position := positionNext;
    velocity := velocityNext;
    covariance := covarianceNext;
    initialized := initializedNext;
    rejections := rejectionsNext;
    rejectionElapsed := rejectionElapsedNext_s;
    reseedDisabledOk := reseedDisabledOk and not reseededOut
      and correctionOutcome <> Estimation.StrapdownINS.CorrectionReseeded
      and abs(position[1]) < 1.0;

    (positionNext, velocityNext, quaternionNext, gyroscopeBiasNext,
     accelerometerBiasNext, covarianceNext, initializedNext, rejectionsNext,
     rejectionElapsedNext_s, gpsOk, recoveryStage, correctionOutcome,
     estimateValid, reseededOut) := gpsTick(
      initialized, position, velocity, quaternion, gyroscopeBias,
      accelerometerBias, covariance, rejections, rejectionElapsed,
      farGps, farGpsVelocity, true, true, 3.0 * dt, dt, gate,
      reseedInflate, reseedDivergent, 0.0, anchorPrev);
    position := positionNext;
    velocity := velocityNext;
    covariance := covarianceNext;
    initialized := initializedNext;
    rejections := rejectionsNext;
    rejectionElapsed := rejectionElapsedNext_s;
    reseedDisabledOk := reseedDisabledOk and not reseededOut
      and correctionOutcome <> Estimation.StrapdownINS.CorrectionReseeded
      and abs(position[1]) < 1.0;

    (positionNext, velocityNext, quaternionNext, gyroscopeBiasNext,
     accelerometerBiasNext, covarianceNext, initializedNext, rejectionsNext,
     rejectionElapsedNext_s, gpsOk, recoveryStage, correctionOutcome,
     estimateValid, reseededOut) := gpsTick(
      initialized, position, velocity, quaternion, gyroscopeBias,
      accelerometerBias, covariance, rejections, rejectionElapsed,
      farGps, farGpsVelocity, true, true, 4.0 * dt, dt, gate,
      reseedInflate, reseedDivergent, 0.0, anchorPrev);
    position := positionNext;
    velocity := velocityNext;
    covariance := covarianceNext;
    initialized := initializedNext;
    rejections := rejectionsNext;
    rejectionElapsed := rejectionElapsedNext_s;
    reseedDisabledOk := reseedDisabledOk and not reseededOut
      and correctionOutcome <> Estimation.StrapdownINS.CorrectionReseeded
      and abs(position[1]) < 1.0;

    (positionNext, velocityNext, quaternionNext, gyroscopeBiasNext,
     accelerometerBiasNext, covarianceNext, initializedNext, rejectionsNext,
     rejectionElapsedNext_s, gpsOk, recoveryStage, correctionOutcome,
     estimateValid, reseededOut) := gpsTick(
      initialized, position, velocity, quaternion, gyroscopeBias,
      accelerometerBias, covariance, rejections, rejectionElapsed,
      farGps, farGpsVelocity, true, true, 5.0 * dt, dt, gate,
      reseedInflate, reseedDivergent, 0.0, anchorPrev);
    position := positionNext;
    velocity := velocityNext;
    covariance := covarianceNext;
    initialized := initializedNext;
    rejections := rejectionsNext;
    rejectionElapsed := rejectionElapsedNext_s;
    reseedDisabledOk := reseedDisabledOk and not reseededOut
      and correctionOutcome <> Estimation.StrapdownINS.CorrectionReseeded
      and abs(position[1]) < 1.0;

    (positionNext, velocityNext, quaternionNext, gyroscopeBiasNext,
     accelerometerBiasNext, covarianceNext, initializedNext, rejectionsNext,
     rejectionElapsedNext_s, gpsOk, recoveryStage, correctionOutcome,
     estimateValid, reseededOut) := gpsTick(
      initialized, position, velocity, quaternion, gyroscopeBias,
      accelerometerBias, covariance, rejections, rejectionElapsed,
      farGps, farGpsVelocity, true, true, 6.0 * dt, dt, gate,
      reseedInflate, reseedDivergent, 0.0, anchorPrev);
    position := positionNext;
    covariance := covarianceNext;
    reseedDisabledOk := reseedDisabledOk and not reseededOut
      and correctionOutcome <> Estimation.StrapdownINS.CorrectionReseeded
      and abs(position[1]) < 1.0;

    assert(reseedFireOk,
      "Stage 3 did not re-seed the state onto the persistent fix at the "
      + "configured window, or did not restore the position and velocity "
      + "covariance while keeping attitude and clearing the clock");
    assert(reseedDisabledOk,
      "The default (disabled) re-seed window re-seeded the state or "
      + "adopted the rejected fix");

    assert(boundsOk,
      "Bounded covariance propagation exceeded the position limit");
    assert(startupOk,
      "Startup initialization did not leave a clean health state");
    assert(rejectionOk,
      "Gate rejection did not advance the rejection clock by one sample "
      + "interval per tick, moved the estimate, or fired the recovery "
      + "ladder before the inflate window elapsed");
    assert(inflateOk,
      "Stage 1 did not ramp position covariance by one bounded step "
      + "while leaving position, velocity, attitude and the attitude "
      + "covariance untouched");
    assert(recoveryOk,
      "A fix consistent with the ramped covariance was not admitted, or "
      + "did not contract the position variance");
    passed := true;
  end run;