Estimation.FusionHorizon.HorizonEstimator

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

block HorizonEstimator
  "One fusion horizon, any strapdown filter, an inertial-rate published state"

  replaceable block FilterModel = Estimation.StrapdownINS.ESKF.Estimator
    constrainedby Estimation.StrapdownINS.PartialEstimator
    "The filter that runs AT the horizon. It is swapped by redeclaration and
     nothing else in this block changes: the buffer, the predictor, the
     re-base, and the published boundary are shared, which is what makes a
     comparison between two filters a comparison of the filters";

  // The rate lattice is STRUCTURAL. Both buffers this block composes -- the
  // delta ring and the five measurement queues -- take their lengths from
  // these three numbers, and a buffer length is an array dimension. It has to
  // carry a value when the code is generated, so it is evaluated at
  // translation rather than left tunable, here as well as inside each
  // sub-block: a tunable parameter forwarded into a structural one is still
  // tunable, and the dimension it feeds becomes unevaluable.
  parameter Real samplePeriod(unit = "s", min = 1.0e-9) = 0.00125
    "Inertial and output-predictor tick"
    annotation(Evaluate = true);
  parameter Real fusionPeriod_s(unit = "s", min = 1.0e-9) = 0.01
    "Filter release interval, one step per composed horizon packet"
    annotation(Evaluate = true);
  parameter Real fusionHorizon_s(unit = "s", min = 0.0) = 0.2
    annotation(Evaluate = true);
  parameter Real gravityWorldEnu_m_s2[3] = {0.0, 0.0, -9.81};
  parameter Boolean useFirstOrderHold = true;
  parameter Real initialPositionWorldEnu_m[3] = zeros(3);
  parameter Real initialVelocityWorldEnu_m_s[3] = zeros(3);
  parameter Real initialQuaternionWorldBody[4] = {1.0, 0.0, 0.0, 0.0};
  parameter Real initialGyroscopeBiasBodyFlu_rad_s[3] = zeros(3);
  parameter Real initialAccelerometerBiasBodyFlu_m_s2[3] = zeros(3);
  parameter Real mocapPeriod_s(unit = "s", min = 1.0e-9) = 0.01
    annotation(Evaluate = true);
  parameter Real gpsPeriod_s(unit = "s", min = 1.0e-9) = 0.1
    annotation(Evaluate = true);
  parameter Real magnetometerPeriod_s(unit = "s", min = 1.0e-9) = 0.05
    annotation(Evaluate = true);
  parameter Real barometerPeriod_s(unit = "s", min = 1.0e-9) = 0.02
    annotation(Evaluate = true);
  parameter Real opticalFlowPeriod_s(unit = "s", min = 1.0e-9) = 0.01
    "Shortest interval each aiding source is declared to deliver at. It sizes
     that source's delayed-measurement queue and nothing else, and it is
     structural for the reason recorded on samplePeriod"
    annotation(Evaluate = true);
  parameter Real correctionRateBudget_hz(unit = "1/s", min = 0.0) =
    1.0 / fusionPeriod_s
    "Shifted fusion instants per second this composition can produce, which is
     what the predictor must recompose for. It is the FUSION RATE, not a
     sensor rate, and that is the fact the delayed horizon changes.

     On the live-edge path the predictor's budget was charged a nominal 5 Hz,
     documented as covering a 5 Hz GPS fix rate. Behind the queues that number
     is wrong in kind. Every measurement the horizon reaches is ripe, the
     filter accepts at most one per tick and can accept on every tick, and the
     aiding set of this vehicle offers a candidate on essentially every tick,
     so the worst case and very nearly the typical case is one shifted instant
     per fusion period.

     Left as an expression rather than a number so it cannot drift from the
     release rate it is derived from. It is forwarded to the predictor, whose
     existing budget assertion then compares it against the measured fold
     budget -- and at the flight lattice that comparison FAILS, which is the
     honest outcome and is discussed in Estimation.FusionHorizon.HorizonEstimator documentation.";
  parameter Real maximumSourceDelay_s(unit = "s", min = 0.0) = 0.11
    "Worst end-to-end age any aiding source is declared to deliver at. The
     horizon must cover it with headroom; see
     Estimation.FusionHorizon.AidingBuffer, which owns the relation and asserts
     it";
  parameter Real horizonJitterMargin_s(unit = "s", min = 0.0) = 0.05;
  parameter Real maximumResidualAge_s(unit = "s", min = 0.0) = fusionPeriod_s
    "How far the fusion instant may stand past a measurement's own timestamp
     and still fuse it. See Estimation.FusionHorizon.AidingBuffer, which owns
     this bound and publishes the residual actually achieved";

  input Boolean reset;
  input Real angularVelocityMeasuredBodyFlu_rad_s[3](each unit = "rad/s");
  input Real specificForceMeasuredBodyFlu_m_s2[3](each unit = "m/s2");
  Avionics.MocapSampleInput mocap;
  Avionics.GpsSampleInput gps;
  Avionics.MagnetometerSampleInput magnetometer;
  Avionics.BarometerSampleInput barometer;
  Avionics.OpticalFlowSampleInput opticalFlow;

  // TWO STATES, NAMED APART. Once the filter fuses at t - D the block holds
  // two different answers to "where is the vehicle", and they are 200 ms and
  // one horizon of motion apart. There is deliberately NO field called
  // `estimate` any more: a consumer cannot pick one by accident, because
  // there is nothing generic to pick.
  //
  // Which one a consumer wants is not a matter of taste. Control and guidance
  // must have the predicted state or they fly a horizon behind the vehicle,
  // and anything that LATCHES the estimate on a mode change -- a trajectory
  // carrot's bumpless entry is the case in this corpus -- must have it too,
  // because latching the horizon state starts the new mode from where the
  // vehicle was D ago and the entry jumps by D times ground speed. At the
  // flight horizon and 3 m/s that is 0.6 m of commanded step at the instant a
  // pilot changes mode.
  //
  // The horizon state is what the covariance describes, so consistency
  // analysis, NEES, and any log line that is compared against a covariance
  // must use IT and not the predicted one. Using the predicted state against
  // the filter's covariance compares a state the covariance does not describe.
  discrete Avionics.NavigationEstimateOutput predictedEstimate
    "The state at NOW, republished every inertial tick. THE CONTROL PATH: this
     is what guidance, control, and any mode-entry latch consume.";
  discrete Avionics.NavigationEstimateOutput horizonEstimate
    "The filter's own state at the fusion instant t - D, republished at the
     inertial rate and stamped with the fusion instant rather than with now.
     NOT FOR CONTROL. It is the state the filter's covariance describes, so it
     is what a consistency or NEES evaluation must be computed against, and it
     is what a log line carrying a covariance beside it must quote.";
  discrete output Boolean horizonReady(start = false, fixed = true);
  discrete output Boolean rebased(start = false, fixed = true);
  discrete output Integer bufferedDeltaCount(start = 0, fixed = true);
  discrete output Boolean biasMoveExceeded(start = false, fixed = true)
    "The filter's bias moved further from the horizon's anchor than the
     first-order Jacobian move is declared good for. A supervision signal, not
     a gate: the state is published as computed";
  discrete output Real worstAidingResidualAge_s(
    unit = "s", start = 0.0, fixed = true)
    "Largest residual any aiding measurement has been delivered to the filter
     with. Bounded by maximumResidualAge_s by construction and published so
     the bound is observable rather than argued: it is the whole quantitative
     claim of fusing at a horizon";
  discrete output Boolean aidingRefused(start = false, fixed = true)
    "A measurement was refused at arrival for being later than the horizon,
     discarded at delivery for the same reason, or displaced from a full
     queue, on this tick";
  discrete output Integer aidingRefusedLateCount(start = 0, fixed = true)
    "Measurements the fusion instant had already passed when they arrived. On
     a horizon longer than every source's transport latency this stays zero,
     and a nonzero value is a statement about the horizon length rather than
     about the filter";

  Estimation.FusionHorizon.OutputPredictor horizon(
    samplePeriod=samplePeriod,
    fusionPeriod_s=fusionPeriod_s,
    fusionHorizon_s=fusionHorizon_s,
    gravityWorldEnu_m_s2=gravityWorldEnu_m_s2,
    useFirstOrderHold=useFirstOrderHold,
    initialGyroscopeBiasAnchorBodyFlu_rad_s=
      initialGyroscopeBiasBodyFlu_rad_s,
    correctionRateBudget_hz=correctionRateBudget_hz,
    initialAccelerometerBiasAnchorBodyFlu_m_s2=
      initialAccelerometerBiasBodyFlu_m_s2,
    initialPositionWorldEnu_m=initialPositionWorldEnu_m,
    initialVelocityWorldEnu_m_s=initialVelocityWorldEnu_m_s,
    initialQuaternionWorldBody=initialQuaternionWorldBody);

  // THE OTHER HALF OF A DELAYED HORIZON. Moving the filter's epoch back to
  // t - D without holding the aiding back with it does not fuse delayed
  // measurements, it fuses measurements from the FUTURE: every sensor packet
  // is stamped ahead of the instant the filter is standing on, and the
  // filter's own timestamp rule rejects a negative age outright. The queues
  // hold each measurement until the fusion instant reaches its timestamp.
  Estimation.FusionHorizon.AidingBuffer aiding(
    samplePeriod=samplePeriod,
    fusionPeriod_s=fusionPeriod_s,
    fusionHorizon_s=fusionHorizon_s,
    mocapPeriod_s=mocapPeriod_s,
    gpsPeriod_s=gpsPeriod_s,
    magnetometerPeriod_s=magnetometerPeriod_s,
    barometerPeriod_s=barometerPeriod_s,
    opticalFlowPeriod_s=opticalFlowPeriod_s,
    maximumSourceDelay_s=maximumSourceDelay_s,
    horizonJitterMargin_s=horizonJitterMargin_s,
    maximumResidualAge_s=maximumResidualAge_s);

  FilterModel filter(
    samplePeriod=fusionPeriod_s,
    // The filter's own timestamp bound IS the queue's residual bound, not the
    // quarter second the live-edge path admits. Behind the queues nothing can
    // reach the filter outside that bound, so this is defence in depth rather
    // than the mechanism; what it buys is that a future change which bypasses
    // a queue is refused by timestamp instead of quietly transporting a
    // measurement Jacobian a quarter of a second to meet the state.
    maximumAidingDelay_s=maximumResidualAge_s,
    gravityWorldEnu_m_s2=gravityWorldEnu_m_s2,
    initialPositionWorldEnu_m=initialPositionWorldEnu_m,
    initialVelocityWorldEnu_m_s=initialVelocityWorldEnu_m_s,
    initialQuaternionWorldBody=initialQuaternionWorldBody,
    initialGyroscopeBiasBodyFlu_rad_s=initialGyroscopeBiasBodyFlu_rad_s,
    initialAccelerometerBiasBodyFlu_m_s2=
      initialAccelerometerBiasBodyFlu_m_s2);

protected
  discrete Boolean horizonValid(start = false, fixed = true);
  discrete Boolean horizonShifted(start = false, fixed = true);
  discrete Real horizonPosition_m[3](each start = 0.0, each fixed = true);
  discrete Real horizonVelocity_m_s[3](each start = 0.0, each fixed = true);
  discrete Real horizonQuaternion[4](
    start = {1.0, 0.0, 0.0, 0.0}, each fixed = true);
  discrete Real horizonGyroscopeBias_rad_s[3](
    each start = 0.0, each fixed = true);
  discrete Real horizonAccelerometerBias_m_s2[3](
    each start = 0.0, each fixed = true);
  discrete Boolean filterValidHeld(start = false, fixed = true);
  discrete Boolean filterShiftedHeld(start = false, fixed = true);
  discrete Integer filterCorrectionCountHeld(start = 0, fixed = true)
    "The accepted-correction count as of the previous inertial tick. The edge
     the re-base fires on is a CHANGE in this number, which is the only
     well-defined edge across the rate change between the filter and here.";
  discrete Real filterPositionHeld_m[3](each start = 0.0, each fixed = true);
  discrete Real filterVelocityHeld_m_s[3](each start = 0.0, each fixed = true);
  discrete Real filterQuaternionHeld[4](
    start = {1.0, 0.0, 0.0, 0.0}, each fixed = true);
  discrete Real filterGyroscopeBiasHeld_rad_s[3](
    each start = 0.0, each fixed = true);
  discrete Real filterAccelerometerBiasHeld_m_s2[3](
    each start = 0.0, each fixed = true);

algorithm
  when sample(0.0, samplePeriod) then
    // The buffer produces the packet the filter consumes and the filter
    // produces the horizon state the buffer re-bases onto. Both clauses fire
    // together at every release, so the causal order is stated here rather
    // than left for a scheduler to pick: the horizon reads what the filter
    // published, one inertial tick after it published it. That is the right
    // epoch, not a compromise -- the release advanced the buffer tail to the
    // new fusion instant on the same tick the filter advanced its state to it.
    // Every history read is a pre() inside this clause, so the one tick of
    // delay is written where it happens instead of appearing as a pre() in a
    // continuous equation, which OpenModelica cannot sort against a clocked
    // producer.
    horizonValid := pre(filterValidHeld);
    horizonShifted := pre(filterShiftedHeld);
    horizonPosition_m := pre(filterPositionHeld_m);
    horizonVelocity_m_s := pre(filterVelocityHeld_m_s);
    horizonQuaternion := pre(filterQuaternionHeld);
    horizonGyroscopeBias_rad_s := pre(filterGyroscopeBiasHeld_rad_s);
    horizonAccelerometerBias_m_s2 := pre(filterAccelerometerBiasHeld_m_s2);
  end when;

algorithm
  when sample(0.0, samplePeriod) then
    // A separate clause, and that is the causal split, not a formatting
    // choice. The clause above reads only pre() and feeds the horizon; this one
    // reads the filter, which the horizon feeds. Written as one clause the two
    // halves would sit in a current-value cycle that no scheduler can order.
    filterValidHeld := filter.estimate.valid;
    // EDGE, not level. filter.status.correctionOutcome is a LEVEL: it stands
    // for the whole 100 Hz filter tick, which is eight inertial ticks here, so
    // reading it directly fired a re-base on all eight and turned one accepted
    // correction into eight full folds. The WCET record carries what that did
    // to the correction-rate ceiling.
    //
    // A rising edge on the level would not do either: back-to-back accepted
    // corrections hold the level true across the filter-tick boundary, and the
    // second correction would never reach the predictor. So the boundary
    // carries a monotonic accepted-correction count and the edge is a change
    // in it. Avionics.EstimatorStatus.acceptedCorrectionCount was added for
    // this and its documentation records why the level was the wrong signal.
    filterShiftedHeld := filter.status.acceptedCorrectionCount
      <> pre(filterCorrectionCountHeld);
    filterCorrectionCountHeld := filter.status.acceptedCorrectionCount;
    filterPositionHeld_m := filter.estimate.positionWorldEnu_m;
    filterVelocityHeld_m_s := filter.estimate.velocityWorldEnu_m_s;
    filterQuaternionHeld := filter.estimate.quaternionWorldBody;
    filterGyroscopeBiasHeld_rad_s := filter.gyroscopeBiasBodyFlu_rad_s;
    filterAccelerometerBiasHeld_m_s2 := filter.accelerometerBiasBodyFlu_m_s2;
    (predictedEstimate.positionWorldEnu_m,
     predictedEstimate.velocityWorldEnu_m_s,
     predictedEstimate.accelerationWorldEnu_m_s2,
     predictedEstimate.quaternionWorldBody,
     predictedEstimate.rotationWorldBody,
     predictedEstimate.eulerRpy_rad,
     predictedEstimate.angularVelocityWorldEnu_rad_s) :=
      Estimation.FusionHorizon.navigationEstimate(
        Estimation.FusionHorizon.Pose(
          positionWorldEnu_m=horizon.positionWorldEnu_m,
          velocityWorldEnu_m_s=horizon.velocityWorldEnu_m_s,
          quaternionWorldBody=horizon.quaternionWorldBody),
        horizon.angularVelocityBodyFlu_rad_s,
        specificForceMeasuredBodyFlu_m_s2,
        filterAccelerometerBiasHeld_m_s2,
        gravityWorldEnu_m_s2);
    predictedEstimate.valid := filter.estimate.valid;
    predictedEstimate.timestamp_s := time;
    predictedEstimate.angularVelocityBodyFlu_rad_s :=
      horizon.angularVelocityBodyFlu_rad_s;

    // The horizon state, published from the SAME latched values the re-base
    // composes onto. Deliberately not re-read from the filter here: one read
    // of the filter's pose per tick, used for both purposes, so the state the
    // predictor was built on and the state published for analysis cannot
    // disagree. If that read is ever wrong, both go wrong together and the
    // published state shows it, which is better than a re-base quietly
    // composing onto something nobody can see.
    (horizonEstimate.positionWorldEnu_m,
     horizonEstimate.velocityWorldEnu_m_s,
     horizonEstimate.accelerationWorldEnu_m_s2,
     horizonEstimate.quaternionWorldBody,
     horizonEstimate.rotationWorldBody,
     horizonEstimate.eulerRpy_rad,
     horizonEstimate.angularVelocityWorldEnu_rad_s) :=
      Estimation.FusionHorizon.navigationEstimate(
        Estimation.FusionHorizon.Pose(
          positionWorldEnu_m=filterPositionHeld_m,
          velocityWorldEnu_m_s=filterVelocityHeld_m_s,
          quaternionWorldBody=filterQuaternionHeld),
        horizon.angularVelocityBodyFlu_rad_s,
        specificForceMeasuredBodyFlu_m_s2,
        filterAccelerometerBiasHeld_m_s2,
        gravityWorldEnu_m_s2);
    horizonEstimate.valid := filter.estimate.valid and horizon.horizonReady;
    // STAMPED WITH THE FUSION INSTANT, not with now. That is what makes a
    // mis-wiring visible in any log rather than only in flight: the two
    // published states carry timestamps a horizon apart, so a consumer that
    // took the wrong one is reading a state whose own timestamp says so.
    horizonEstimate.timestamp_s := horizon.horizonPacket.timestamp_s;
    // The rate AT THE HORIZON, not at now: the released window's own mean
    // rate, corrected by the bias the filter believes. Publishing the live
    // rate here would put a now-epoch quantity inside a horizon-epoch record
    // and defeat the point of separating them.
    horizonEstimate.angularVelocityBodyFlu_rad_s :=
      horizon.horizonPacket.angularVelocityBodyFlu_rad_s
      - filterGyroscopeBiasHeld_rad_s;
    horizonReady := horizon.horizonReady;
    rebased := horizon.rebased;
    bufferedDeltaCount := horizon.bufferedDeltaCount;
    biasMoveExceeded := horizon.biasMoveExceeded;
    worstAidingResidualAge_s := aiding.worstDeliveredAge_s;
    aidingRefused := aiding.aidingRefused;
    aidingRefusedLateCount := aiding.refusedLateCount;
  end when;

equation
  horizon.reset = reset;
  horizon.angularVelocityMeasuredBodyFlu_rad_s =
    angularVelocityMeasuredBodyFlu_rad_s;
  horizon.specificForceMeasuredBodyFlu_m_s2 =
    specificForceMeasuredBodyFlu_m_s2;
  horizon.horizonStateValid = horizonValid;
  horizon.horizonStateShifted = horizonShifted;
  horizon.horizonPositionWorldEnu_m = horizonPosition_m;
  horizon.horizonVelocityWorldEnu_m_s = horizonVelocity_m_s;
  horizon.horizonQuaternionWorldBody = horizonQuaternion;
  horizon.horizonGyroscopeBiasBodyFlu_rad_s = horizonGyroscopeBias_rad_s;
  horizon.horizonAccelerometerBiasBodyFlu_m_s2 = horizonAccelerometerBias_m_s2;

  // The queues are clocked with the predictor and pulsed by it. The epoch is
  // the one the predictor stamps on the inertial packet it releases, and it
  // has to be the same number: the two packets the filter consumes on one
  // tick name one fusion instant, which is the entire content of fusing at a
  // horizon. Reading it off the packet rather than recomputing it is what
  // makes that identity structural instead of a coincidence between two
  // arithmetic expressions.
  aiding.reset = reset;
  aiding.horizonValid = horizon.horizonReady;
  aiding.horizonEpoch_s = horizon.horizonPacket.timestamp_s;
  aiding.horizonReleased = horizon.horizonPacket.valid;
  aiding.mocap = mocap;
  aiding.gps = gps;
  aiding.magnetometer = magnetometer;
  aiding.barometer = barometer;
  aiding.opticalFlow = opticalFlow;

  filter.reset = reset;
  // Whole-record equalities rather than connect equations. A connection set
  // whose only members are one outer input and one inner input leaves
  // OpenModelica unable to sort the result against the clocked producers on
  // either side.
  //
  // The aiding streams no longer pass straight through. Each one now reaches
  // the filter from its queue, at the fusion instant its own timestamp names,
  // which is what makes the filter's measurement age a residual inside one
  // release window rather than the whole transport latency of the sensor.
  // EVERY source routes through the same path, mocap included: the
  // asymmetry where mocap was aged like the others and aligned like nothing
  // else closes here by construction rather than by adding a fifth copy of a
  // retrodiction stanza.
  filter.imu = horizon.horizonPacket;
  filter.mocap = aiding.mocapAtHorizon;
  filter.gps = aiding.gpsAtHorizon;
  filter.magnetometer = aiding.magnetometerAtHorizon;
  filter.barometer = aiding.barometerAtHorizon;
  filter.opticalFlow = aiding.opticalFlowAtHorizon;

  annotation(Documentation(info = "<html>
    <p>Buffer aiding measurements, run the selected filter at a delayed fusion
    instant, then predict its corrected state to the present. The default
    filter is the ESKF; alternatives implement
    <a href=\"modelica://Estimation.StrapdownINS.PartialEstimator\">PartialEstimator</a>.</p>
    <h4>Output epochs</h4>
    <p><code>predictedEstimate</code> is the current-time state for control and
    guidance. <code>horizonEstimate</code> is the delayed state described by
    the filter covariance. Use the latter and its timestamp for NEES and
    covariance diagnostics. No matching current-time covariance is propagated.</p>
    <h4>Timing and delivery</h4>
    <p>Default intervals are 1.25 ms for IMU sampling, 10 ms for fusion and
    200 ms for horizon lag. The fusion interval must contain an integral number
    of IMU intervals; the horizon must contain an integral number of fusion
    intervals and cover <code>maximumSourceDelay_s</code> plus
    <code>horizonJitterMargin_s</code>. Set each source's period to its shortest
    delivery interval so the bounded queues have sufficient capacity.</p>
    <p>Use capture timestamps and one-tick fresh pulses. Wait for
    <code>horizonReady</code>; monitor <code>aidingRefused</code>,
    <code>aidingRefusedLateCount</code>, <code>worstAidingResidualAge_s</code>
    and <code>biasMoveExceeded</code>. The remaining measurement-age alignment
    is bounded by <code>maximumResidualAge_s</code>.</p>
    <h4>Runtime budget</h4>
    <p>The default 100 Hz correction budget exceeds the predictor's default
    7.3 Hz fold budget and fails its budget assertion. Configure the budget
    from measurements of generated code on the intended target; increasing
    the parameter alone does not improve runtime. Accepted corrections can
    shift the state at every fusion release, even with slower individual
    sensors.</p>
    </html>"));
end HorizonEstimator;