Estimation.StrapdownINS.ESKF.Estimator

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

block Estimator
  "Sampled aided strapdown inertial-navigation ESKF"
  extends Estimation.StrapdownINS.PartialEstimator;

  parameter Boolean useSemiDirectBias = false;
  parameter Boolean useStationaryImu = false;
  parameter Boolean useSquareRootCovariance = false;
  output Real errorCovarianceRoot[15, 15](each start=0, each fixed=true);
  output Boolean stationaryImuCorrectionAccepted(start = false, fixed = true);
  input Boolean vehicleAtRest = false;
  parameter Boolean useDeclaredRestBarometerCalibration = false;
  parameter Boolean useBarometerBiasConsider = false;
  parameter Boolean useJointBarometerBias = false;
  output Real barometerBiasCrossCovariance[15](each start=0, each fixed=true);
  parameter Real stationaryVelocityVariance_m2_s2(unit = "m2/s2") = 0.01;

  parameter Boolean useEquivariantMagnetometer = false
    "Fuse a calibrated local magnetic field vector with symmetric SO(3) linearization";
  parameter Boolean useGeometricAlignment = false
    "Condition startup uncertainty on the vectors used for quiet alignment";

  // Consistency instrumentation (NEES/NIS study): mirror the full
  // error-state covariance and the bias estimates as public outputs so
  // a mission-level consistency evaluation can be post-processed from
  // exported channels without touching the filter internals.
  output Real errorCovariance[15, 15](
    each start = 0.0, each fixed = true)
    "Full local-error tangent covariance, mirrored each estimator tick";
  output Real estimatedGyroscopeBias_rad_s[3](
    each start = 0.0, each fixed = true)
    "Gyroscope bias estimate, mirrored each estimator tick";
  output Real estimatedAccelerometerBias_m_s2[3](
    each start = 0.0, each fixed = true)
    "Accelerometer bias estimate, mirrored each estimator tick";

  parameter VarianceLimits varianceLimits =
    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))
    "Diagonal covariance growth bounds from the state units and mission
     envelope: position sigma 100 m dwarfs the sub-100 m RDD2 flight
     volume; velocity sigma 20 m/s is beyond the airframe speed
     envelope; attitude sigma sqrt(10) ~ pi rad means orientation fully
     unknown; gyro-bias sigma 0.1 rad/s and accelerometer-bias sigma
     1 m/s2 (~0.1 g) bound consumer-MEMS turn-on bias. At these values
     the state is already maximally uninformative, so capping there
     loses no information while keeping the covariance inside the
     dynamic range a binary32 Cholesky can factor.";
  parameter Real innovationGate(unit = "1") = 6.0
    "Chi-square innovation gate per degree of freedom; non-positive
     disables. NIS is chi-square with m degrees of freedom for a
     consistent filter; 6.0 per dof keeps the false-rejection
     probability at or below ~0.25% for the m = 2, 3, 6 corrections
     used here (chi-square tails: 12 @ m=2 -> 0.25%, 18 @ m=3 ->
     0.044%, 36 @ m=6 -> 2.8e-6).";
  // The recovery-window parameters are validated both at the public block
  // boundary and inside the pure step function. The duplicated contract is
  // intentional: invalid configuration must disable recovery rather than
  // silently selecting an unsafe recovery mode.
  parameter Real covarianceInflateWindow_s(unit = "s") = 1.0
    "Unbroken wall-clock time during which the anchor source has been
     unable to move the state, after which the position and velocity
     covariance begins ramping toward the mission envelope with the state
     left untouched. Long enough that a short outlier burst cannot provoke
     it, short enough that an over-tight covariance is reopened well
     before dead-reckoning drift matters.

     This is a TIME, not a count of rejected corrections. The count it
     replaces was documented as 'aiding streams at 20-100 Hz, so 50
     rejections spans roughly 0.5-2.5 s' -- but nothing tied the count to
     the aiding rate, and at the 1 kHz rate this block is actually wired
     at, with aiding asserted every tick, 50 rejections was 51 ms:
     10-50x faster than the latency the value was chosen for. It is
     advanced on estimator ticks while the anchor holds without accepting,
     so it measures elapsed time at every wiring and aiding rate, and in
     particular under the pulsed sensor wiring the deployed GPS driver
     actually produces.";
  parameter Real covarianceInflateTimeConstant_s(unit = "s")
      = 0.5
    "e-folding time of the covariance ramp once the window has elapsed.

     The ramp exists because the alternative -- setting the covariance
     straight to the mission envelope -- converts gating into adoption:
     at envelope variance against a typical reported measurement noise the
     Kalman gain is 0.9998, so the next sample simply becomes the state.
     Measured with that jump in place, and strictly worse than taking no
     action at all: a cold-start filter against a truthful 8-12 m/s stream
     acquired tens of m/s of spurious velocity and +52 m of position
     error, and the discrepancy at which a lying sole optical-flow sensor
     was adopted rather than rejected fell from 3.63 m/s to 0.72 m/s.

     0.5 s doubles a variance about every 0.35 s, so a gate locked out by
     a factor of ten reopens in roughly a second while any single tick can
     widen it by at most a factor of 1.004 at 1 kHz. That bound is the
     point: the gain available immediately after inflation cannot swing
     the state outside the pre-inflation gate.";
  parameter Real aidingDivergentWindow_s(unit = "s") = 5.0
    "Unbroken wall-clock time after which the anchor is REPORTED
     divergent on status.recoveryStage. Five times the inflate window: if
     ramping the covariance to the full mission envelope has not let the
     anchor back in within that span, the disagreement is larger than the
     envelope the vehicle is declared to operate in.

     This raises a status flag and nothing else. It deliberately does NOT
     withdraw estimate.valid: on the deployed stack that clears
     RatesValid, zeroes the motors and latches the fault in every mode
     including ACRO, so it is a crash rather than the failsafe handover it
     was intended to be. The wrapper maps this stage onto a non-latching
     mode demotion.";
  parameter Real aidingStaleTimeout_s(unit = "s") = 0.5
    "How long a source may go without a fresh sample before it stops
     counting as available for anchor selection.

     Sensor `valid` cannot be read as availability on a single tick. The
     deployed GPS driver pulses `valid` with `fresh`, asserting it only on
     the ticks that carry a fix, so a rule keyed on `valid` sees GPS as
     absent for 99 of every 100 ticks at 10 Hz. The mocap wrapper has the
     opposite property -- it derives `valid` from record contents and
     `fresh` from subscription updates, independently, so a source can be
     permanently valid and never fresh. 0.5 s spans ten intervals of the
     slowest aiding rate this block is specified for while still demoting
     a genuinely stalled source well inside the divergence window.";
  parameter Real aidingReseedWindow_s(unit = "s") = 0.0
    "Unbroken wall-clock time the anchor source has been unable to move the
     state after which, on the next fresh finite anchor sample, the
     estimator re-seeds position (and velocity from a GPS sample that
     reports it) from that sample and restores the position and velocity
     covariance to the initial variances. It keeps attitude and the bias
     states.

     DISABLED BY DEFAULT: non-positive turns re-seeding off, which is the
     value carried here so every pinned test and witness trace keeps its
     behaviour. When enabled it must be finite and exceed
     aidingDivergentWindow_s, so a re-seed happens only after the gentler
     covariance-inflation stage has been given the whole divergence window
     to readmit the anchor on its own. A deployment that wants automatic
     recovery from a sustained outage sets this positive on the extends
     clause, as Vehicles.Rdd2.NavigationEstimator does. An unusable value
     disables only the re-seed and leaves the rest of the ladder in force.";
  parameter Real initialAlignmentWindow_s(unit = "s") = 0.0
    "Unbroken quasi-static time the IMU must report before the filter aligns
     its initial attitude from the specific force. A vehicle that boots while
     being carried, or whose inertial stream has not settled, is not leveled
     on that sample; it stays uninitialized until it has rested this long.
     Without GNSS or mocap nothing later can observe a tilt taken at
     alignment, which is how a wrong start becomes an unbounded velocity.
     DISABLED BY DEFAULT: non-positive aligns on the first usable sample, the
     value carried here so every pinned test and witness trace keeps its
     behaviour. Vehicles.Rdd2.NavigationEstimator sets it on the extends
     clause. When the window is enabled a filter without a magnetometer
     levels on the specific force with the heading left at zero instead of
     taking the configured initial quaternion.";
  parameter Real initialAlignmentTimeout_s(unit = "s") = 0.0
    "Time with a usable IMU after which a pending alignment is taken on the
     current sample even though the vehicle never held still for the window,
     so a vibrating airframe or an out-of-scale sensor still gets a filter.
     Non-positive waits indefinitely. Only meaningful with
     initialAlignmentWindow_s enabled; RDD2 uses 5 s.";
  parameter Real quietSpecificForceTolerance_m_s2(unit = "m/s2") = 0.5
    "How far the specific-force magnitude may differ from gravity while the
     vehicle still counts as quasi-static: about 0.05 g, above MEMS noise and
     motor vibration at rest, below any real acceleration.";
  parameter Real quietAngularRateLimit_rad_s(unit = "rad/s") = 0.1
    "Angular-rate magnitude below which the vehicle counts as quasi-static,
     about six degrees per second.";
  parameter Real quietFilterTimeConstant_s(unit = "s") = 0.15
    "Time constant of the first-order low-pass the quasi-static test and the
     accelerometer alignment read the IMU through. A resting vehicle whose
     sensor puts several m/s2 of noise on each sample still passes the test
     once the filter has settled, about three time constants after the first
     usable sample. Non-positive reads the raw samples.";
  parameter Real pseudoPositionVariance_m2(unit = "m2") = 0.0
    "Per-axis variance of the synthetic hold-position measurement fused on
     every tick that no anchor source is live and no real sensor was fused.
     It holds the filter to the last position it had while aided (or the
     initialization position) with a large uncertainty, the fake-position
     fusion of the PX4 and ArduPilot filters, so a tilt error cannot
     integrate into an unbounded velocity that gates every later measurement
     out. DISABLED BY DEFAULT: non-positive turns it off. RDD2 uses 100
     (10 m sigma).";
  parameter Real zeroVelocityVariance_m2_s2(unit = "m2/s2") = 0.0
    "Per-axis variance of the synthetic zero-velocity measurement fused while
     the vehicle is quasi-static and no anchor source is live. Through the
     velocity-attitude cross covariance this is the update that makes a tilt
     error observable on a vehicle at rest with nothing else to aid it.
     DISABLED BY DEFAULT: non-positive turns it off. RDD2 uses 0.09 (0.3 m/s
     sigma), loose enough that a slowly drifting unaided hover is not pinned.";

protected
  discrete Real statePosition[3](each start = 0.0, each fixed = true);
  discrete Real stateVelocity[3](each start = 0.0, each fixed = true);
  discrete Real stateQuaternion[4](
    start = {1.0, 0.0, 0.0, 0.0}, each fixed = true);
  discrete Real stateGyroscopeBias[3](each start = 0.0, each fixed = true);
  discrete Real stateAccelerometerBias[3](each start = 0.0, each fixed = true);
  discrete Real stateCovarianceRoot[15, 15](each start=0, each fixed=true);
  discrete Real stateCovariance[15, 15](each start = 0.0, each fixed = true);
  Real barometerObservationBiasVariance_m2;
  Real barometerObservationAge_s;
  discrete Boolean initialized(start = false, fixed = true);
  discrete Boolean predictionAccepted(start = false, fixed = true);
  discrete Boolean mocapCorrectionAccepted(start = false, fixed = true);
  discrete Boolean gpsPositionCorrectionAccepted(start = false, fixed = true);
  discrete Boolean gpsVelocityCorrectionAccepted(start = false, fixed = true);
  discrete Boolean magnetometerCorrectionAccepted(start = false, fixed = true);
  discrete Boolean barometerCorrectionAccepted(start = false, fixed = true);
  discrete Boolean opticalFlowCorrectionAccepted(start = false, fixed = true);
  discrete Boolean barometerBiasInitialized(start = false, fixed = true);
  discrete Boolean barometerBiasUpdateAccepted(start = false, fixed = true);
  discrete Integer barometerBiasCalibrationCount(start = 0, fixed = true);
  discrete Boolean barometerBiasCalibrationClosed(start = false, fixed = true);
  discrete Real stateBarometerBias_m(start = 0.0, fixed = true);
  discrete Real stateBarometerBiasVariance_m2(start = 4.0, fixed = true);
  discrete Boolean terrainInitialized(start = false, fixed = true);
  discrete Real stateTerrainAltitude_m(start = 0.0, fixed = true);
  discrete Real stateTerrainVariance_m2(start = 4.0, fixed = true);
  discrete Boolean terrainCorrectionAccepted(start = false, fixed = true);
  discrete Real auxiliaryRotationWorldBody[3, 3](each start=0.0, each fixed=true);
  discrete Real auxiliaryPredictedVariance_m2(start=1.0, fixed=true);
  discrete Real auxiliaryObservationVariance_m2(start=1.0, fixed=true);
  discrete Real auxiliaryInnovationVariance_m2(start=1.0, fixed=true);
  discrete Real auxiliaryGain(start=0.0, fixed=true);
  discrete Real auxiliaryObservation_m(start=0.0, fixed=true);
  discrete Real auxiliaryCosTilt(start=1.0, fixed=true);
  discrete Integer consecutiveRejectedCorrections(start = 0, fixed = true);
  discrete Real rejectionElapsed_s(start = 0.0, fixed = true);
  discrete Integer mocapRejections(start = 0, fixed = true);
  discrete Integer gpsRejections(start = 0, fixed = true);
  discrete Integer opticalFlowRejections(start = 0, fixed = true);
  discrete Integer recoveryStage(start = 0, fixed = true);
  discrete Integer correctionOutcome(start = 0, fixed = true);
  discrete Integer correctionSource(start = 0, fixed = true);
  discrete Integer acceptedCorrectionCount(start = 0, fixed = true)
    "Monotonic count of SHIFTED FUSION INSTANTS on the status boundary: ticks
     on which at least one aiding correction was accepted, at most one per
     tick. See Avionics.EstimatorStatus.acceptedCorrectionCount, which states
     the contract. Coalesce all real-sensor acceptance flags into one increment
     even when several independent sources correct the state on the same tick.

     The outcome field is a level held for the whole filter tick, so a
     consumer running faster than the filter cannot edge-detect it; the count
     gives that consumer a well-defined edge.";
  discrete Real normalizedInnovationSquared(start = 0.0, fixed = true);
  discrete Boolean estimateValid(start = false, fixed = true);
  discrete Boolean reseeded(start = false, fixed = true);
  discrete Integer reseedCount(start = 0, fixed = true)
    "Monotonic count of automatic recovery-ladder re-seeds since the last
     reset; incremented on each tick reseeded is true";
  discrete Integer anchorSource(start = 0, fixed = true);
  discrete Real mocapStale_s(start = 1.0e30, fixed = true);
  discrete Real gpsStale_s(start = 1.0e30, fixed = true);
  discrete Real opticalFlowStale_s(start = 1.0e30, fixed = true);
  discrete Real imuAngularVelocityHeld_rad_s[3](each start = 0.0, each fixed = true);
  discrete Real imuSpecificForceHeld_m_s2[3](each start = 0.0, each fixed = true);
  discrete Real imuTimestampHeld_s(start = 0.0, fixed = true);
  discrete Boolean imuPayloadHeld(start = false, fixed = true);
  discrete Real mocapTimestampConsumed_s(start = -1.0e30, fixed = true);
  discrete Real gpsTimestampConsumed_s(start = -1.0e30, fixed = true);
  discrete Real magnetometerTimestampConsumed_s(start = -1.0e30, fixed = true);
  discrete Real barometerTimestampConsumed_s(start = -1.0e30, fixed = true);
  discrete Real opticalFlowTimestampConsumed_s(start = -1.0e30, fixed = true);
  discrete Real barometerBiasTimestampConsumed_s(
    start = -1.0e30, fixed = true);
  discrete Real terrainTimestampConsumed_s(start = -1.0e30, fixed = true);
  discrete Real quietElapsed_s(start = 0.0, fixed = true);
  discrete Real alignmentWait_s(start = 0.0, fixed = true);
  discrete Real pseudoPositionHold_m[3](each start = 0.0, each fixed = true);
  discrete Integer alignmentSource(start = 0, fixed = true);
  discrete Real alignmentSpecificForce_m_s2[3](each start = 0.0, each fixed = true);
  discrete Real quietAngularRate_rad_s[3](each start = 0.0, each fixed = true);
  discrete Boolean pseudoPositionCorrectionAccepted(start = false, fixed = true);
  discrete Boolean zeroVelocityCorrectionAccepted(start = false, fixed = true);

algorithm
  when sample(0.0, samplePeriod) then
    barometerBiasTimestampConsumed_s := if not reset
        and barometer.valid
        and abs(barometer.timestamp_s)
          < FiniteMagnitudeLimit
        and barometer.timestamp_s
          > pre(barometerBiasTimestampConsumed_s) + 1.0e-9
      then barometer.timestamp_s
      elseif reset then -1.0e30
      else pre(barometerBiasTimestampConsumed_s);
    terrainTimestampConsumed_s := if not reset
        and opticalFlow.valid
        and abs(opticalFlow.timestamp_s)
          < FiniteMagnitudeLimit
        and opticalFlow.timestamp_s
          > pre(terrainTimestampConsumed_s) + 1.0e-9
      then opticalFlow.timestamp_s
      elseif reset then -1.0e30
      else pre(terrainTimestampConsumed_s);
    auxiliaryRotationWorldBody := LieGroups.SO3.Quat.to_DCM(
      pre(stateQuaternion));
    auxiliaryPredictedVariance_m2 := max(
      if not reset and (pre(barometerBiasCalibrationCount) > 0
          or (useJointBarometerBias and pre(initialized))
          or (useDeclaredRestBarometerCalibration
            and pre(barometerBiasInitialized))) then
        pre(stateBarometerBiasVariance_m2)
      else initialBarometerBiasVariance_m2, 1.0e-12)
      + max(barometerBiasProcessNoise_m2_s, 0.0) * samplePeriod;
    auxiliaryObservationVariance_m2 := barometer.variance_m2;
    barometerObservationAge_s := imu.timestamp_s - barometer.timestamp_s;
    barometerObservationBiasVariance_m2 := auxiliaryPredictedVariance_m2;
    if useBarometerBiasConsider or useJointBarometerBias then
      barometerObservationBiasVariance_m2 := auxiliaryPredictedVariance_m2
        - max(barometerBiasProcessNoise_m2_s, 0.0)
          * max(barometerObservationAge_s, 0.0);
    end if;
    auxiliaryInnovationVariance_m2 := barometerObservationBiasVariance_m2
      + auxiliaryObservationVariance_m2;
    auxiliaryObservation_m := barometer.altitudeWorldEnu_m
      - initialPositionWorldEnu_m[3];
    barometerBiasCalibrationClosed := not reset
      and (pre(barometerBiasCalibrationClosed)
        or (useDeclaredRestBarometerCalibration and not vehicleAtRest));
    // Average a finite stationary startup window against the declared local
    // origin. During this window the sample is withheld from the navigation
    // correction, avoiding the positive feedback caused by using the same
    // pressure observation to update both altitude and its datum.
    if not reset and barometer.valid
        and ((useDeclaredRestBarometerCalibration
            and vehicleAtRest and not barometerBiasCalibrationClosed)
          or (not useDeclaredRestBarometerCalibration
            and not pre(barometerBiasInitialized)))
        and abs(barometer.timestamp_s)
          < FiniteMagnitudeLimit
        and barometer.timestamp_s
          > pre(barometerBiasTimestampConsumed_s) + 1.0e-9
        and abs(barometer.altitudeWorldEnu_m) < FiniteMagnitudeLimit
        and barometer.variance_m2 > 0.0
        and barometer.variance_m2 < FiniteMagnitudeLimit
        and (not (useBarometerBiasConsider or useJointBarometerBias)
          or (barometerObservationAge_s >= -1.0e-6
            and barometerObservationAge_s <= maximumAidingDelay_s
            and barometerObservationBiasVariance_m2 >= 0.0
            and barometerObservationBiasVariance_m2 < FiniteMagnitudeLimit))
        and auxiliaryInnovationVariance_m2 > 1.0e-12 then
      auxiliaryGain := barometerObservationBiasVariance_m2
        / auxiliaryInnovationVariance_m2;
      stateBarometerBias_m := (if not reset
          and pre(barometerBiasCalibrationCount) > 0
          then pre(stateBarometerBias_m)
        else initialBarometerBias_m) + auxiliaryGain
          * (auxiliaryObservation_m - (if not reset
              and pre(barometerBiasCalibrationCount) > 0
              then pre(stateBarometerBias_m)
            else initialBarometerBias_m));
      stateBarometerBiasVariance_m2 := max((1.0 - auxiliaryGain)
        * auxiliaryPredictedVariance_m2, 1.0e-12);
      if useBarometerBiasConsider or useJointBarometerBias then
        stateBarometerBiasVariance_m2 := max((1.0 - auxiliaryGain)
          * barometerObservationBiasVariance_m2
          + max(barometerBiasProcessNoise_m2_s, 0.0)
            * max(barometerObservationAge_s, 0.0), 1.0e-12);
      end if;
      barometerBiasCalibrationCount :=
        pre(barometerBiasCalibrationCount) + 1;
      barometerBiasInitialized := pre(barometerBiasCalibrationCount) + 1
        >= barometerBiasCalibrationSamples;
      barometerBiasUpdateAccepted := true;
    else
      stateBarometerBias_m := if not reset
          and (pre(barometerBiasCalibrationCount) > 0
            or (useJointBarometerBias and pre(initialized)))
        then pre(stateBarometerBias_m) else initialBarometerBias_m;
      stateBarometerBiasVariance_m2 := auxiliaryPredictedVariance_m2;
      barometerBiasInitialized := not reset
        and (pre(barometerBiasInitialized)
          or (useDeclaredRestBarometerCalibration
            and barometerBiasCalibrationClosed));
      barometerBiasCalibrationCount := if reset then 0
        else pre(barometerBiasCalibrationCount);
      barometerBiasUpdateAccepted := false;
    end if;

    auxiliaryCosTilt := auxiliaryRotationWorldBody[3, 3];
    auxiliaryPredictedVariance_m2 := max(
      if not reset and pre(terrainInitialized) then
        pre(stateTerrainVariance_m2) else initialTerrainVariance_m2,
      1.0e-12) + max(terrainProcessNoise_m2_s, 0.0) * samplePeriod;
    auxiliaryObservation_m := pre(statePosition[3])
      - opticalFlow.groundDistance_m * auxiliaryCosTilt;
    auxiliaryObservationVariance_m2 := auxiliaryCosTilt * auxiliaryCosTilt
        * opticalFlow.groundDistanceVariance_m2
      + pre(stateCovariance[1, 1]) + pre(stateCovariance[2, 2])
      + pre(stateCovariance[3, 3])
      + opticalFlow.groundDistance_m * opticalFlow.groundDistance_m
        * (pre(stateCovariance[7, 7]) + pre(stateCovariance[8, 8]));
    auxiliaryInnovationVariance_m2 := auxiliaryPredictedVariance_m2
      + auxiliaryObservationVariance_m2;
    if not reset and opticalFlow.valid
        and abs(opticalFlow.timestamp_s)
          < FiniteMagnitudeLimit
        and opticalFlow.timestamp_s
          > pre(terrainTimestampConsumed_s) + 1.0e-9
        and opticalFlow.quality >= minimumOpticalFlowQuality
        and opticalFlow.groundDistance_m > 0.05
        and opticalFlow.groundDistanceVariance_m2 > 0.0
        and auxiliaryCosTilt >= minimumRangeCosTilt
        and auxiliaryInnovationVariance_m2 > 1.0e-12 then
      auxiliaryGain := auxiliaryPredictedVariance_m2
        / auxiliaryInnovationVariance_m2;
      stateTerrainAltitude_m := (if not reset and pre(terrainInitialized)
          then pre(stateTerrainAltitude_m)
        else initialTerrainAltitudeWorldEnu_m) + auxiliaryGain
          * (auxiliaryObservation_m - (if not reset and pre(terrainInitialized)
              then pre(stateTerrainAltitude_m)
            else initialTerrainAltitudeWorldEnu_m));
      stateTerrainVariance_m2 := max((1.0 - auxiliaryGain)
        * auxiliaryPredictedVariance_m2, 1.0e-12);
      terrainInitialized := true;
      terrainCorrectionAccepted := true;
    else
      stateTerrainAltitude_m := if not reset and pre(terrainInitialized)
        then pre(stateTerrainAltitude_m) else initialTerrainAltitudeWorldEnu_m;
      stateTerrainVariance_m2 := auxiliaryPredictedVariance_m2;
      terrainInitialized := not reset and pre(terrainInitialized);
      terrainCorrectionAccepted := false;
    end if;
    (statePosition,
     stateVelocity,
     stateQuaternion,
     stateGyroscopeBias,
     stateAccelerometerBias,
     stateCovariance,
     initialized,
     predictionAccepted,
     mocapCorrectionAccepted,
     gpsPositionCorrectionAccepted,
     gpsVelocityCorrectionAccepted,
     magnetometerCorrectionAccepted,
     barometerCorrectionAccepted,
     opticalFlowCorrectionAccepted,
     consecutiveRejectedCorrections,
     rejectionElapsed_s,
     recoveryStage,
     correctionOutcome,
     correctionSource,
     normalizedInnovationSquared,
     estimateValid,
     reseeded,
     mocapRejections,
     gpsRejections,
     opticalFlowRejections,
     anchorSource,
     mocapStale_s,
     gpsStale_s,
     opticalFlowStale_s,
     imuAngularVelocityHeld_rad_s,
     imuSpecificForceHeld_m_s2,
     imuTimestampHeld_s,
     imuPayloadHeld,
     mocapTimestampConsumed_s,
     gpsTimestampConsumed_s,
     magnetometerTimestampConsumed_s,
     barometerTimestampConsumed_s,
     opticalFlowTimestampConsumed_s,
     alignmentWait_s,
     quietElapsed_s,
     pseudoPositionHold_m,
     alignmentSource,
     alignmentSpecificForce_m_s2,
     quietAngularRate_rad_s,
     pseudoPositionCorrectionAccepted,
     zeroVelocityCorrectionAccepted,
     stationaryImuCorrectionAccepted,
     stateCovarianceRoot,
     barometerBiasCrossCovariance,
     stateBarometerBias_m,
     stateBarometerBiasVariance_m2) :=
      step(
        pre(initialized),
        State(
          positionWorldEnu_m=pre(statePosition),
          velocityWorldEnu_m_s=pre(stateVelocity),
          quaternionWorldBody=pre(stateQuaternion),
          gyroscopeBiasBodyFlu_rad_s=pre(stateGyroscopeBias),
          accelerometerBiasBodyFlu_m_s2=pre(stateAccelerometerBias),
          covariance=pre(stateCovariance),
          covarianceRoot=pre(stateCovarianceRoot),
          barometerBiasCrossCovariance=pre(barometerBiasCrossCovariance),
          barometerBias_m=stateBarometerBias_m,
          barometerBiasVariance_m2=stateBarometerBiasVariance_m2,
          useJointBarometerBias=useJointBarometerBias,
          useSquareRootCovariance=useSquareRootCovariance),
        reset,
        imu,
        mocap,
        gps,
        magnetometer,
        Avionics.BarometerSample(
          valid=barometer.valid and pre(barometerBiasInitialized)
            and not barometerBiasUpdateAccepted
            and barometer.timestamp_s
              > pre(barometerBiasTimestampConsumed_s) + 1.0e-9,
          fresh=barometer.fresh,
          timestamp_s=barometer.timestamp_s,
          altitudeWorldEnu_m=barometer.altitudeWorldEnu_m,
          variance_m2=barometer.variance_m2),
        opticalFlow,
        gravityWorldEnu_m_s2,
        samplePeriod,
        Tuning(
          initialState=NominalState(
            positionWorldEnu_m=initialPositionWorldEnu_m,
            velocityWorldEnu_m_s=initialVelocityWorldEnu_m_s,
            quaternionWorldBody=initialQuaternionWorldBody,
            gyroscopeBiasBodyFlu_rad_s=initialGyroscopeBiasBodyFlu_rad_s,
            accelerometerBiasBodyFlu_m_s2=
              initialAccelerometerBiasBodyFlu_m_s2),
          initialVariances=initialVariances,
          processNoise=processNoise,
          varianceLimits=varianceLimits,
          innovationGate=innovationGate,
          localMagneticFieldWorldEnu_T=localMagneticFieldWorldEnu_T,
          useEquivariantMagnetometer=useEquivariantMagnetometer,
          useGeometricAlignment=useGeometricAlignment,
          useSemiDirectBias=useSemiDirectBias,
          useStationaryImu=useStationaryImu,
          useSquareRootCovariance=useSquareRootCovariance,
          barometerBias_m=stateBarometerBias_m,
          barometerBiasVariance_m2=stateBarometerBiasVariance_m2,
          useBarometerBiasConsider=useBarometerBiasConsider,
          useJointBarometerBias=useJointBarometerBias,
          barometerBiasProcessNoise_m2_s=barometerBiasProcessNoise_m2_s,
          maximumAidingDelay_s=maximumAidingDelay_s,
          minimumOpticalFlowQuality=minimumOpticalFlowQuality,
          minimumOpticalFlowGroundDistance_m=
            minimumOpticalFlowGroundDistance_m,
          covarianceInflateWindow_s=covarianceInflateWindow_s,
          covarianceInflateTimeConstant_s=covarianceInflateTimeConstant_s,
          aidingDivergentWindow_s=aidingDivergentWindow_s,
          aidingStaleTimeout_s=aidingStaleTimeout_s,
          aidingReseedWindow_s=aidingReseedWindow_s,
          initialAlignmentWindow_s=initialAlignmentWindow_s,
          initialAlignmentTimeout_s=initialAlignmentTimeout_s,
          quietSpecificForceTolerance_m_s2=quietSpecificForceTolerance_m_s2,
          quietAngularRateLimit_rad_s=quietAngularRateLimit_rad_s,
          quietFilterTimeConstant_s=quietFilterTimeConstant_s,
          pseudoPositionVariance_m2=pseudoPositionVariance_m2,
          zeroVelocityVariance_m2_s2=zeroVelocityVariance_m2_s2,
          stationaryVelocityVariance_m2_s2=stationaryVelocityVariance_m2_s2),
        pre(consecutiveRejectedCorrections),
        pre(rejectionElapsed_s),
        pre(mocapRejections),
        pre(gpsRejections),
        pre(opticalFlowRejections),
        pre(anchorSource),
        pre(mocapStale_s),
        pre(gpsStale_s),
        pre(opticalFlowStale_s),
        pre(imuAngularVelocityHeld_rad_s),
        pre(imuSpecificForceHeld_m_s2),
        pre(imuTimestampHeld_s),
        pre(mocapTimestampConsumed_s),
        pre(gpsTimestampConsumed_s),
        pre(magnetometerTimestampConsumed_s),
        pre(barometerTimestampConsumed_s),
        pre(opticalFlowTimestampConsumed_s),
        pre(quietElapsed_s),
        pre(alignmentWait_s),
        pre(pseudoPositionHold_m),
        pre(alignmentSource),
        pre(alignmentSpecificForce_m_s2),
        pre(quietAngularRate_rad_s),
        vehicleAtRest);
    (estimate.valid,
     estimate.timestamp_s,
     estimate.positionWorldEnu_m,
     estimate.velocityWorldEnu_m_s,
     estimate.accelerationWorldEnu_m_s2,
     estimate.quaternionWorldBody,
     estimate.rotationWorldBody,
     estimate.eulerRpy_rad,
     estimate.angularVelocityBodyFlu_rad_s,
     estimate.angularVelocityWorldEnu_rad_s) :=
      navigationEstimate(
        State(
          positionWorldEnu_m=statePosition,
          velocityWorldEnu_m_s=stateVelocity,
          quaternionWorldBody=stateQuaternion,
          gyroscopeBiasBodyFlu_rad_s=stateGyroscopeBias,
          accelerometerBiasBodyFlu_m_s2=stateAccelerometerBias,
          covariance=stateCovariance,
          covarianceRoot=stateCovarianceRoot,
          barometerBiasCrossCovariance=barometerBiasCrossCovariance,
          barometerBias_m=stateBarometerBias_m,
          barometerBiasVariance_m2=stateBarometerBiasVariance_m2,
          useJointBarometerBias=useJointBarometerBias,
          useSquareRootCovariance=useSquareRootCovariance),
        // The HELD sample, never the raw connector: three published fields
        // are computed from the IMU rather than from the state, so passing
        // the raw sample here is what let a non-finite reading reach the
        // payload while prediction was correctly refusing it.
        Avionics.ImuSample(
          valid=imu.valid,
          fresh=imu.fresh,
          timestamp_s=imuTimestampHeld_s,
          angularVelocityBodyFlu_rad_s=imuAngularVelocityHeld_rad_s,
          specificForceBodyFlu_m_s2=imuSpecificForceHeld_m_s2,
          deltaAngleBodyFlu_rad=imu.deltaAngleBodyFlu_rad,
          deltaVelocityBodyFlu_m_s=imu.deltaVelocityBodyFlu_m_s,
          deltaPositionBodyFlu_m=imu.deltaPositionBodyFlu_m,
          deltaQuaternionBodyFlu=imu.deltaQuaternionBodyFlu,
          gyroscopeBiasLinearizationBodyFlu_rad_s=
            imu.gyroscopeBiasLinearizationBodyFlu_rad_s,
          accelerometerBiasLinearizationBodyFlu_m_s2=
            imu.accelerometerBiasLinearizationBodyFlu_m_s2,
          deltaRotationGyroscopeBiasJacobian_s=
            imu.deltaRotationGyroscopeBiasJacobian_s,
          deltaVelocityGyroscopeBiasJacobian_m=
            imu.deltaVelocityGyroscopeBiasJacobian_m,
          deltaVelocityAccelerometerBiasJacobian_s=
            imu.deltaVelocityAccelerometerBiasJacobian_s,
          deltaPositionGyroscopeBiasJacobian_m_s=
            imu.deltaPositionGyroscopeBiasJacobian_m_s,
          deltaPositionAccelerometerBiasJacobian_s2=
            imu.deltaPositionAccelerometerBiasJacobian_s2,
          integrationTime_s=imu.integrationTime_s),
        gravityWorldEnu_m_s2,
        estimateValid);
    acceptedCorrectionCount := if reset then 0
      elseif mocapCorrectionAccepted or gpsPositionCorrectionAccepted
          or gpsVelocityCorrectionAccepted or opticalFlowCorrectionAccepted
          or barometerCorrectionAccepted or magnetometerCorrectionAccepted
          or (vehicleAtRest and zeroVelocityCorrectionAccepted)
          or stationaryImuCorrectionAccepted
        then pre(acceptedCorrectionCount) + 1
      else pre(acceptedCorrectionCount);
    reseedCount := if reset then 0
      elseif reseeded then pre(reseedCount) + 1
      else pre(reseedCount);
    status.initialized := initialized;
    status.predictionAccepted := predictionAccepted;
    status.mocapCorrectionAccepted := mocapCorrectionAccepted;
    status.gpsPositionCorrectionAccepted := gpsPositionCorrectionAccepted;
    status.gpsVelocityCorrectionAccepted := gpsVelocityCorrectionAccepted;
    status.magnetometerCorrectionAccepted := magnetometerCorrectionAccepted;
    status.barometerCorrectionAccepted := barometerCorrectionAccepted;
    status.terrainCorrectionAccepted := terrainCorrectionAccepted;
    status.opticalFlowCorrectionAccepted := opticalFlowCorrectionAccepted;
    status.consecutiveRejectedCorrections := consecutiveRejectedCorrections;
    status.rejectionElapsed_s := rejectionElapsed_s;
    status.mocapConsecutiveRejections := mocapRejections;
    status.gpsConsecutiveRejections := gpsRejections;
    status.opticalFlowConsecutiveRejections := opticalFlowRejections;
    status.correctionOutcome := correctionOutcome;
    status.acceptedCorrectionCount := acceptedCorrectionCount;
    status.correctionSource := correctionSource;
    status.normalizedInnovationSquared := normalizedInnovationSquared;
    status.recoveryStage := recoveryStage;
    status.anchorSource := anchorSource;
    status.imuPayloadHeld := imuPayloadHeld;
    status.reseeded := reseeded;
    status.reseedCount := reseedCount;
    status.alignmentSource := alignmentSource;
    status.alignmentSpecificForceBodyFlu_m_s2 := alignmentSpecificForce_m_s2;
    status.quietElapsed_s := quietElapsed_s;
    status.pseudoPositionCorrectionAccepted := pseudoPositionCorrectionAccepted;
    status.zeroVelocityCorrectionAccepted := zeroVelocityCorrectionAccepted;
    errorCovariance := stateCovariance;
    errorCovarianceRoot := stateCovarianceRoot;
    estimatedGyroscopeBias_rad_s := stateGyroscopeBias;
    estimatedAccelerometerBias_m_s2 := stateAccelerometerBias;
  end when;

equation
  navigationCovarianceLocal = stateCovariance[1:6, 1:6];
  gyroscopeBiasBodyFlu_rad_s = stateGyroscopeBias;
  accelerometerBiasBodyFlu_m_s2 = stateAccelerometerBias;
  barometerBias_m = stateBarometerBias_m;
  barometerBiasVariance_m2 = stateBarometerBiasVariance_m2;
  terrainAltitudeWorldEnu_m = stateTerrainAltitude_m;
  terrainAltitudeVariance_m2 = stateTerrainVariance_m2;
  heightAboveTerrain_m = statePosition[3] - stateTerrainAltitude_m;

  // Misconfiguration must be loud, not silently reinterpreted. The `min`
  // attributes above stop a negative value landing on 0.0, which is the
  // one value that arms the ladder permanently; these state the ordering
  // and finiteness the ladder actually depends on. A NaN window would
  // otherwise disable the ladder silently, since every comparison against
  // it is false -- the same predicate-exhaustion defect this whole change
  // exists to remove, and it must not reappear in the tuning surface.
  assert(covarianceInflateWindow_s > 0.0
    and covarianceInflateWindow_s < FiniteMagnitudeLimit,
    "covarianceInflateWindow_s must be finite and strictly positive");
  assert(covarianceInflateTimeConstant_s > 0.0
    and covarianceInflateTimeConstant_s < FiniteMagnitudeLimit,
    "covarianceInflateTimeConstant_s must be finite and strictly positive");
  assert(aidingStaleTimeout_s > 0.0
    and aidingStaleTimeout_s < FiniteMagnitudeLimit,
    "aidingStaleTimeout_s must be finite and strictly positive");
  assert(aidingDivergentWindow_s > covarianceInflateWindow_s
    and aidingDivergentWindow_s < FiniteMagnitudeLimit,
    "aidingDivergentWindow_s must be finite and exceed covarianceInflateWindow_s");
  // The re-seed is opt-in, so a non-positive window is a legitimate
  // configuration and is not asserted against. A finite POSITIVE window that
  // fails to exceed the divergent window is a configuration error, not a
  // disable, so it is caught loudly here rather than silently ignored.
  assert(not (aidingReseedWindow_s > 0.0
      and aidingReseedWindow_s < FiniteMagnitudeLimit
      and aidingReseedWindow_s <= aidingDivergentWindow_s),
    "aidingReseedWindow_s, when enabled, must exceed aidingDivergentWindow_s");
  assert(quietSpecificForceTolerance_m_s2 > 0.0
    and quietSpecificForceTolerance_m_s2 < FiniteMagnitudeLimit,
    "quietSpecificForceTolerance_m_s2 must be finite and strictly positive");
  assert(quietAngularRateLimit_rad_s > 0.0
    and quietAngularRateLimit_rad_s < FiniteMagnitudeLimit,
    "quietAngularRateLimit_rad_s must be finite and strictly positive");
  assert(quietFilterTimeConstant_s < FiniteMagnitudeLimit,
    "quietFilterTimeConstant_s must be finite");

  annotation(Documentation(info = "<html>
    <p>This block is a concrete implementation of the stable estimator
    contract and can be exported directly as an eFMU. Prediction uses one
    vectorized 15-state covariance, including every cross-axis and bias
    correlation. One fresh aiding packet is accepted per estimator tick with
    deterministic priority mocap, GPS, then optical flow; GPS position and
    velocity are fused jointly when both are valid.</p>
    <p>The SE_2(3) pose/velocity/position part uses geometric propagation,
    injection, and covariance reset. The additive gyroscope and accelerometer
    bias states make the complete augmented system neither strictly invariant
    nor exactly log-linear. It is therefore an aided strapdown INS error-state
    Kalman filter, not a strict invariant-filter construction.</p>
    <p><b>Acceptance is affirmative.</b> A measurement moves the state only
    when it has proven it should: the residual is finite, the innovation
    covariance factors, and the normalized innovation squared is non-negative
    and within the gate. Nothing is accepted merely because no rejection test
    happened to fire, which is what allowed a NaN measurement -- against which
    every comparison is false -- to be applied and to poison the state
    permanently.</p>
    <p><b>Recovery is a ladder timed in wall-clock seconds on the anchor
    source.</b> First the position and velocity covariance is inflated
    toward the declared mission envelope with the state untouched, which is
    the only recovery action that cannot itself be wrong and which readmits
    a good fix that had been locked out by an over-tight covariance. If the
    anchor still does not fit by <code>aidingDivergentWindow_s</code> the
    divergence is reported on <code>status.recoveryStage</code>;
    <code>estimate.valid</code> deliberately stays a statement only about
    whether the published numbers are finite, because clearing it on the
    deployed stack latches a motors-zero fault in every mode. An optional
    third stage, enabled by setting <code>aidingReseedWindow_s</code>
    positive and above the divergent window, re-seeds position (and velocity
    from a GPS sample that reports it) from the next fresh finite anchor
    sample and restores the position and velocity covariance to the initial
    variances, keeping attitude and the bias states; it is reported as
    <code>correctionOutcome == CorrectionReseeded</code>,
    <code>status.reseeded</code>, and <code>status.reseedCount</code>, never
    as a gate acceptance, and is refused on a non-finite payload. It is
    disabled by default. The commanded <code>reset</code> input remains the
    operator-level re-seed of position, attitude, and velocity together.</p>
  <h4>Configure a vehicle</h4>
<p>Set the initial pose and biases, initial variances, continuous process-noise
covariances, gravity and local magnetic field through
<a href=\"modelica://Estimation.StrapdownINS.PartialEstimator\">PartialEstimator</a>.
The default magnetic reference is site-specific. The block's
<code>samplePeriod</code> sets its discrete update interval; feed calibrated
ENU/FLU packets on the inherited sensor connectors.</p>
<h4>Choose the IMU path</h4>
<p>Use <a href=\"modelica://Estimation.StrapdownINS.preintegrateImuStep\">preintegrateImuStep</a>
to accumulate intervals and bias sensitivities into an
<a href=\"modelica://Avionics.ImuSample\">ImuSample</a>. A preintegral carries its
integration duration and bias anchor; its rotation, velocity, position and five
Jacobian blocks describe the same interval. The filter transports it to the
estimated bias with
<a href=\"modelica://Estimation.StrapdownINS.correctPreintegratedImu\">correctPreintegratedImu</a>.
This first-order bias correction is not reintegration of the raw samples.</p>
<h4>Inspect health and uncertainty</h4>
<p>Read per-source acceptance flags, innovation statistics and recovery status
from <code>status</code>. A finite <code>estimate</code> does not establish that
position is trustworthy: use the recovery and aiding-health signals as well.
The full error covariance uses body-local right perturbations, ordered
{position, velocity, attitude, gyroscope bias, accelerometer bias}. Transform
truth errors into those coordinates before computing NEES.</p>
<p>To compare delayed fusion with the direct block, place this same backend in
<a href=\"modelica://Estimation.FusionHorizon.HorizonEstimator\">HorizonEstimator</a>.
Its horizon state and current-time predicted output have different timestamps;
only pair a covariance with the state epoch it describes.</p>
    </html>"));
end Estimator;