Estimation/FusionHorizon/AidingBuffer.mo
Download original .moGitHub source
Documentation: Estimation.FusionHorizon.AidingBuffer
within Estimation.FusionHorizon;
block AidingBuffer
"Bounded per-source measurement queues that hand each aiding packet to the
filter at the fusion instant its own timestamp names"
parameter Real samplePeriod(unit = "s", min = 1.0e-9) = 0.00125
"Inertial tick. Arrivals are admitted at this rate so no sensor pulse is
missed between releases; deliveries happen only on a release";
parameter Real fusionPeriod_s(unit = "s", min = 1.0e-9) = 0.01
"Filter release interval. It is also the width of the sub-window a
delivered measurement can still be offset from the fusion instant by,
which is what bounds maximumResidualAge_s below";
parameter Real fusionHorizon_s(unit = "s", min = 1.0e-9) = 0.2
"Lag of the fusion instant behind now, the same horizon the delta ring is
sized for. It is what each queue has to hold, so it is structural here
for the same reason the ring length is structural there"
annotation(Evaluate = true);
// The declared source rates and the slack are the ONLY tuning surface for
// the queue capacities, and they are structural: a queue length is an array
// dimension, so it has to carry a value when the code is generated rather
// than at run time. Evaluate = true says that, and the alternative is a
// dynamically sized buffer, which is the thing a flight timing record cannot
// be written against and which the code generator is right to refuse.
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.02
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 source is declared to deliver at. It sets that
source's queue capacity and nothing else; a source delivering FASTER than
it says overflows its queue and reports the drop rather than corrupting
anything. A deployment that knows a stream is absent shrinks its queue by
declaring a longer period here, which is one place to state a source's
rate rather than two"
annotation(Evaluate = true);
parameter Integer depthSlack(min = 0) = 2
"Slots each queue carries beyond one horizon of its own source, so a queue
reaching exactly one horizon on the tick the fusion instant starts moving
does not have to drop its oldest entry to accept the next arrival"
annotation(Evaluate = true);
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. THE bound the delayed-aiding argument rests on.
A measurement inside the horizon ripens on the first release at or after
its own timestamp, so the residual is in [0, fusionPeriod_s) by
construction and this bound is never the binding constraint in flight.
What it does is name the two failures at the edges: a packet that arrives
after the fusion instant has already passed it, and a packet that could
not be drained fast enough. Both are refused with a named outcome rather
than fused against a state that has moved past them.
It replaces Estimation.StrapdownINS.PartialEstimator.maximumAidingDelay_s
as the quantity the transport error is charged against, and the two are
not the same size: that one is 0.25 s and admits a cubic-Taylor transport
error the design record puts at 12 to 27 percent, this one is one release
period. At the flight lattice that is 0.01 s against 0.25 s.";
parameter Real maximumSourceDelay_s(unit = "s", min = 0.0) = 0.11
"Worst end-to-end age any aiding source is declared to deliver at. It is
what the horizon has to be long enough to cover, and it is stated rather
than inferred because this block cannot see a sensor: it sees timestamps.
The default is the largest plant-side latency the deployed vehicle
carries, GPS at 110 ms, which is PX4's last shipped EKF2_GPS_DELAY
default. A deployment with a slower source must raise this, and raising it
past the horizon is refused below rather than silently aging that source
out of every fusion.";
parameter Real horizonJitterMargin_s(unit = "s", min = 0.0) = 0.05
"Headroom the horizon must keep beyond the worst declared source delay.
A delay is a nominal, not a bound: a driver that usually delivers at
110 ms occasionally delivers later, and a horizon sized exactly to the
nominal turns every one of those into a refused measurement. 50 ms is
roughly half the GPS delay it is protecting, which is the same order as
the jitter a scheduler and a link contribute together, and the flight
configuration clears the sum by a further 40 ms.
PX4 states the same relation without the margin: EKF2_DELAY_MAX, the
delay between now and its delayed-time horizon, `should be at least as
large as the largest EKF2_XXX_DELAY parameter`. This is that rule with
headroom, and it is an assertion rather than a note.";
parameter Real epochTolerance_s(unit = "s", min = 0.0) = 1.0e-7
"Slack on the ripeness comparison. A measurement stamped AT the fusion
instant has to be ripe at it, and a carried epoch advanced by repeated
addition does not land on a sensor's timestamp exactly.
It is an order of magnitude BELOW the 1e-6 s at which the filters reject
a negatively aged measurement, and that ordering is the point rather than
a coincidence. The filter recomputes the age itself from the same two
numbers, so a measurement this block admits as ripe within its tolerance
reaches the filter with an age no more negative than that tolerance; if
the two were equal the outcome would sit on a knife edge and a
measurement could be released here and refused there with nothing saying
why. Both are far below the 10 ms release period and far above the
representation error of either quantity over a flight.";
// ---- queue capacities ---------------------------------------------------
// A whole horizon of each source, rounded up, plus the slack. It is worth
// saying why the answer is the WHOLE horizon rather than the part of it a
// measurement actually waits out. In steady flight a measurement stamped
// t_m reaches the driver at t_m + L and ripens when the fusion instant
// reaches t_m, at t_m + D, so it waits D - L and the queue stands at
// (D - L) / sourcePeriod entries. STARTUP is the worse case and it is the
// one the capacity has to cover: the fusion instant does not begin advancing
// until the delta ring has filled, so everything a source delivers during
// that first horizon is queued at once, which is D / sourcePeriod entries.
//
// Written inline rather than as a call to a named function, and that is a
// COMPILER ACCOMMODATION rather than a preference, recorded so it can be
// reverted. Rumoca 0.10.0 folds this arithmetic in an array-dimension
// position but does not fold a user function call in the same position once
// the block is a SUB-COMPONENT: standalone the call folds, composed it
// reports the dimension unevaluable. The two forms compute the same integer
// and the named one read better.
final parameter Integer mocapDepth(min = 2) =
integer(ceil(fusionHorizon_s / mocapPeriod_s - 1.0e-9)) + depthSlack;
final parameter Integer gpsDepth(min = 2) =
integer(ceil(fusionHorizon_s / gpsPeriod_s - 1.0e-9)) + depthSlack;
final parameter Integer magnetometerDepth(min = 2) =
integer(ceil(fusionHorizon_s / magnetometerPeriod_s - 1.0e-9)) + depthSlack;
final parameter Integer barometerDepth(min = 2) =
integer(ceil(fusionHorizon_s / barometerPeriod_s - 1.0e-9)) + depthSlack;
final parameter Integer opticalFlowDepth(min = 2) =
integer(ceil(fusionHorizon_s / opticalFlowPeriod_s - 1.0e-9)) + depthSlack
"Queue capacities, derived from the horizon and each source's declared
rate. Final rather than overridable, because a capacity and the rate it
is derived from must not be settable independently: a queue declared
shorter than the rate beside it implies would drop measurements on the
ground and report a source rate that nothing in the model contradicts.
Nothing here is allocated at run time -- every capacity is fixed at
translation, every walk is fixed-length, and every index wraps at most
once";
input Boolean reset;
input Boolean horizonValid
"A fusion instant exists. Before the first release the epoch below is
still its seed value, so nothing may be measured against it and nothing
is delivered";
input Real horizonEpoch_s(unit = "s")
"Timestamp of the fusion instant the filter is standing on. The same
number Estimation.FusionHorizon.OutputPredictor stamps on the inertial
packet it releases, and it must be: the two packets the filter consumes
on one tick have to name one instant, which is the whole content of
fusing at a horizon";
input Boolean horizonReleased
"The horizon handed a window over on this tick. Deliveries are pulsed onto
this signal so an aiding packet and the inertial packet reach the filter
together";
Avionics.MocapSampleInput mocap;
Avionics.GpsSampleInput gps;
Avionics.MagnetometerSampleInput magnetometer;
Avionics.BarometerSampleInput barometer;
Avionics.OpticalFlowSampleInput opticalFlow;
discrete Avionics.MocapSampleOutput mocapAtHorizon;
discrete Avionics.GpsSampleOutput gpsAtHorizon;
discrete Avionics.MagnetometerSampleOutput magnetometerAtHorizon;
discrete Avionics.BarometerSampleOutput barometerAtHorizon;
discrete Avionics.OpticalFlowSampleOutput opticalFlowAtHorizon
"The aiding streams as the filter sees them: each packet held back until
the fusion instant reaches its own timestamp, then delivered PULSED on
the release tick. valid and fresh are the same boolean, as they are on
the inertial packet and for the same reason -- a measurement either was
handed over on this tick or was not";
discrete output Real worstDeliveredAge_s(
unit = "s", start = 0.0, fixed = true)
"Largest residual any source has been delivered with since the last reset.
The quantity the filter still transports its measurement Jacobian over,
published so the bound is observable rather than argued";
discrete output Integer refusedLateCount(start = 0, fixed = true)
"Measurements refused at arrival because the fusion instant had already
passed them. On a correctly sized horizon this stays at zero, and a
nonzero value says the horizon is shorter than the transport latency of
some source, which is a configuration fact rather than a filter fault";
discrete output Integer droppedStaleCount(start = 0, fixed = true)
"Queued measurements discarded at delivery for the same reason";
discrete output Integer refusedOverflowCount(start = 0, fixed = true)
"Arrivals refused because the queue was full. Nonzero means a source is
delivering faster than its declared period, so its capacity is short. The
queue keeps the entries it already holds, which are the ones the fusion
instant is about to reach; see AidingRefusedOverflow for why that is the
right way round for a delayed queue and the wrong way round for a
live-edge one";
discrete output Integer deliveredCount(start = 0, fixed = true)
"Measurements handed to the filter since the last reset, across every
source. Published because every other supervision signal here is a count
of something going WRONG, and a queue that delivers nothing satisfies all
of them: without this a silent source and a healthy one are the same
reading";
discrete output Boolean deliveryOutOfOrder(start = false, fixed = true)
"LATCHED. Some source delivered a measurement whose timestamp did not
advance on the one before it. The queues are FIFOs and the fusion instant
advances monotonically, so this cannot happen; it is checked here, beside
the data, because it is an invariant of this block and because a consumer
cannot check it -- the aiding connectors carry one packet at a time and
the ordering is a property of the sequence.";
discrete output Boolean deliveryAfterHorizon(start = false, fixed = true)
"LATCHED. Some source delivered a measurement the fusion instant had NOT
yet reached, which is a measurement from the filter's own future. This is
the delayed-fusion contract itself, and it is asserted where the epoch and
the timestamp are both in hand rather than reconstructed downstream.";
discrete output Integer beforeHorizonCount(start = 0, fixed = true)
"Samples presented before the first release. NOT a refusal and not part of
aidingRefused: the horizon costs one horizon of start-up and this counts
it. Published so a reader can tell a start-up window from a source that
is genuinely arriving late, which is the distinction the aggregate
counters exist to preserve";
discrete output Boolean aidingRefused(start = false, fixed = true)
"Any of the three above happened on this tick. A level for one tick, so a
supervisor sees the event rather than having to difference the counts";
protected
discrete Real mocapQueue[mocapDepth, MocapMeasurementLength](
each start = 0.0, each fixed = true);
discrete Integer mocapHead(start = 1, fixed = true);
discrete Integer mocapTail(start = 1, fixed = true);
discrete Integer mocapCount(start = 0, fixed = true);
discrete Real mocapAdmitted_s(start = -TimestampMagnitudeLimit, fixed = true)
"Timestamp of the last sample admitted from this source. Novelty by
timestamp is the exactly-once handshake across the sensor and inertial
clocks, the same rule the filters use on the same records: a level-held
sample is admitted on the first tick it is presented and on no other";
discrete Real mocapStoredRow[MocapMeasurementLength](
each start = 0.0, each fixed = true);
discrete Integer mocapStoreSlot(start = 0, fixed = true);
discrete Real mocapDeliveredRow[MocapMeasurementLength](
each start = 0.0, each fixed = true);
discrete Real mocapAge_s(start = 0.0, fixed = true);
discrete Boolean mocapDelivered(start = false, fixed = true);
discrete Integer mocapArrival(start = 0, fixed = true);
discrete Integer mocapDelivery(start = 0, fixed = true);
discrete Real gpsQueue[gpsDepth, GpsMeasurementLength](
each start = 0.0, each fixed = true);
discrete Integer gpsHead(start = 1, fixed = true);
discrete Integer gpsTail(start = 1, fixed = true);
discrete Integer gpsCount(start = 0, fixed = true);
discrete Real gpsAdmitted_s(start = -TimestampMagnitudeLimit, fixed = true);
discrete Real gpsStoredRow[GpsMeasurementLength](
each start = 0.0, each fixed = true);
discrete Integer gpsStoreSlot(start = 0, fixed = true);
discrete Real gpsDeliveredRow[GpsMeasurementLength](
each start = 0.0, each fixed = true);
discrete Real gpsAge_s(start = 0.0, fixed = true);
discrete Boolean gpsDelivered(start = false, fixed = true);
discrete Integer gpsArrival(start = 0, fixed = true);
discrete Integer gpsDelivery(start = 0, fixed = true);
discrete Real magnetometerQueue[
magnetometerDepth, MagnetometerMeasurementLength](
each start = 0.0, each fixed = true);
discrete Integer magnetometerHead(start = 1, fixed = true);
discrete Integer magnetometerTail(start = 1, fixed = true);
discrete Integer magnetometerCount(start = 0, fixed = true);
discrete Real magnetometerAdmitted_s(
start = -TimestampMagnitudeLimit, fixed = true);
discrete Real magnetometerStoredRow[MagnetometerMeasurementLength](
each start = 0.0, each fixed = true);
discrete Integer magnetometerStoreSlot(start = 0, fixed = true);
discrete Real magnetometerDeliveredRow[MagnetometerMeasurementLength](
each start = 0.0, each fixed = true);
discrete Real magnetometerAge_s(start = 0.0, fixed = true);
discrete Boolean magnetometerDelivered(start = false, fixed = true);
discrete Integer magnetometerArrival(start = 0, fixed = true);
discrete Integer magnetometerDelivery(start = 0, fixed = true);
discrete Real barometerQueue[barometerDepth, BarometerMeasurementLength](
each start = 0.0, each fixed = true);
discrete Integer barometerHead(start = 1, fixed = true);
discrete Integer barometerTail(start = 1, fixed = true);
discrete Integer barometerCount(start = 0, fixed = true);
discrete Real barometerAdmitted_s(
start = -TimestampMagnitudeLimit, fixed = true);
discrete Real barometerStoredRow[BarometerMeasurementLength](
each start = 0.0, each fixed = true);
discrete Integer barometerStoreSlot(start = 0, fixed = true);
discrete Real barometerDeliveredRow[BarometerMeasurementLength](
each start = 0.0, each fixed = true);
discrete Real barometerAge_s(start = 0.0, fixed = true);
discrete Boolean barometerDelivered(start = false, fixed = true);
discrete Integer barometerArrival(start = 0, fixed = true);
discrete Integer barometerDelivery(start = 0, fixed = true);
discrete Real opticalFlowQueue[
opticalFlowDepth, OpticalFlowMeasurementLength](
each start = 0.0, each fixed = true);
discrete Integer opticalFlowHead(start = 1, fixed = true);
discrete Integer opticalFlowTail(start = 1, fixed = true);
discrete Integer opticalFlowCount(start = 0, fixed = true);
discrete Real opticalFlowAdmitted_s(
start = -TimestampMagnitudeLimit, fixed = true);
discrete Real opticalFlowStoredRow[OpticalFlowMeasurementLength](
each start = 0.0, each fixed = true);
discrete Integer opticalFlowStoreSlot(start = 0, fixed = true);
discrete Real opticalFlowDeliveredRow[OpticalFlowMeasurementLength](
each start = 0.0, each fixed = true);
discrete Real opticalFlowAge_s(start = 0.0, fixed = true);
discrete Boolean opticalFlowDelivered(start = false, fixed = true);
discrete Integer opticalFlowArrival(start = 0, fixed = true);
discrete Integer opticalFlowDelivery(start = 0, fixed = true);
// The unpacked payload is landed in ORDINARY VARIABLES first and copied to
// the connector field by field afterwards. Naming connector components as
// the outputs of a multiple-output function call does not survive
// OpenModelica 1.27 here: measured, every Real member so assigned published
// zero while the row the call was given was correct and every scalar the
// same block computed from that row was correct. The failure is silent,
// which is what makes it worth a comment rather than a shrug -- a queue
// that delivers a valid packet full of zeros passes every ordering and
// residual assertion written about it.
discrete Real mocapOutTimestamp_s;
discrete Real mocapOutPosition_m[3];
discrete Real mocapOutQuaternion[4];
discrete Real mocapOutPositionCovariance_m2[3, 3];
discrete Real mocapOutAttitudeCovariance_rad2[3, 3];
discrete Real gpsOutTimestamp_s;
discrete Boolean gpsOutPositionValid;
discrete Boolean gpsOutVelocityValid;
discrete Real gpsOutGeodetic_deg_m[3];
discrete Real gpsOutPosition_m[3];
discrete Real gpsOutVelocity_m_s[3];
discrete Real gpsOutPositionCovariance_m2[3, 3];
discrete Real gpsOutVelocityCovariance_m2_s2[3, 3];
discrete Real magnetometerOutTimestamp_s;
discrete Real magnetometerOutField_T[3];
discrete Real magnetometerOutCovariance_T2[3, 3];
discrete Real barometerOutTimestamp_s;
discrete Real barometerOutAltitude_m;
discrete Real barometerOutVariance_m2;
discrete Real opticalFlowOutTimestamp_s;
discrete Real opticalFlowOutLineOfSight_rad[2];
discrete Real opticalFlowOutLineOfSightCovariance_rad2[2, 2];
discrete Real opticalFlowOutGyroscope_rad[3];
discrete Real opticalFlowOutGyroscopeCovariance_rad2[3, 3];
discrete Real opticalFlowOutIntegrationTime_s;
discrete Real opticalFlowOutGroundDistance_m;
discrete Real opticalFlowOutGroundDistanceVariance_m2;
discrete Real opticalFlowOutQuality;
// A DELIVERED PACKET IS HELD UNTIL THE NEXT ONE REPLACES IT, and only
// `fresh` is pulsed. Pulsing `valid` too was wrong and the reason is worth
// keeping: a filter fuses at most ONE correction per tick, chosen from a
// priority chain, so on a tick where several sources are ripe together the
// ones the chain does not pick are simply not consumed. With `valid` pulsed
// they were gone by the next tick and the measurement was lost with no
// outcome recorded anywhere. The live-edge wiring never had this problem
// because a driver holds `valid` as a level and pulses `fresh` beside it,
// and the filters' novelty-by-timestamp rule then consumes each packet
// exactly once whenever the chain gets to it. Holding restores that, and it
// is the deployed GPS driver's own waveform.
discrete Real mocapHeldRow[MocapMeasurementLength](
each start = 0.0, each fixed = true);
discrete Boolean mocapHeldValid(start = false, fixed = true);
discrete Real gpsHeldRow[GpsMeasurementLength](
each start = 0.0, each fixed = true);
discrete Boolean gpsHeldValid(start = false, fixed = true);
discrete Real magnetometerHeldRow[MagnetometerMeasurementLength](
each start = 0.0, each fixed = true);
discrete Boolean magnetometerHeldValid(start = false, fixed = true);
discrete Real barometerHeldRow[BarometerMeasurementLength](
each start = 0.0, each fixed = true);
discrete Boolean barometerHeldValid(start = false, fixed = true);
discrete Real opticalFlowHeldRow[OpticalFlowMeasurementLength](
each start = 0.0, each fixed = true);
discrete Boolean opticalFlowHeldValid(start = false, fixed = true);
discrete Real mocapLastDelivered_s(
start = -TimestampMagnitudeLimit, fixed = true);
discrete Real gpsLastDelivered_s(
start = -TimestampMagnitudeLimit, fixed = true);
discrete Real magnetometerLastDelivered_s(
start = -TimestampMagnitudeLimit, fixed = true);
discrete Real barometerLastDelivered_s(
start = -TimestampMagnitudeLimit, fixed = true);
discrete Real opticalFlowLastDelivered_s(
start = -TimestampMagnitudeLimit, fixed = true);
discrete Real mocapArrivedRow[MocapMeasurementLength];
discrete Real gpsArrivedRow[GpsMeasurementLength];
discrete Real magnetometerArrivedRow[MagnetometerMeasurementLength];
discrete Real barometerArrivedRow[BarometerMeasurementLength];
discrete Real opticalFlowArrivedRow[OpticalFlowMeasurementLength];
discrete Boolean mocapArrived(start = false, fixed = true);
discrete Boolean gpsArrived(start = false, fixed = true);
discrete Boolean magnetometerArrived(start = false, fixed = true);
discrete Boolean barometerArrived(start = false, fixed = true);
discrete Boolean opticalFlowArrived(start = false, fixed = true);
algorithm
when sample(0.0, samplePeriod) then
// Five queues, one kernel, five copies of the same twelve lines. The
// repetition is not an accident and it is not a loop waiting to be
// written: the five sources pack to five different widths, so a loop over
// them would need one width for all five and would carry a mocap-sized
// row for a barometer. Everything that could differ between the five is in
// stepQueue, which is written once.
//
// ARRIVALS ARE ADMITTED AT THE INERTIAL RATE, deliveries only on a
// release. A sensor pulse that lands between two releases must not be
// missed, and novelty by timestamp makes admitting on every tick the same
// as admitting once.
// ---- motion capture ---------------------------------------------------
mocapArrivedRow := Estimation.FusionHorizon.packMocap(mocap);
mocapArrived := (not reset) and mocap.valid
and abs(mocap.timestamp_s) < TimestampMagnitudeLimit
and mocap.timestamp_s > pre(mocapAdmitted_s) + 1.0e-9;
(mocapStoreSlot,
mocapStoredRow,
mocapHead,
mocapTail,
mocapCount,
mocapDeliveredRow,
mocapAge_s,
mocapDelivered,
mocapArrival,
mocapDelivery) := Estimation.FusionHorizon.stepQueue(
reset, pre(mocapQueue), pre(mocapHead), pre(mocapTail), pre(mocapCount),
mocapArrived, mocapArrivedRow, horizonReleased, horizonValid,
horizonEpoch_s, maximumResidualAge_s, epochTolerance_s);
mocapQueue := Estimation.FusionHorizon.storeMeasurement(
pre(mocapQueue), mocapStoreSlot, mocapStoredRow);
// Advanced on ADMISSION rather than on delivery, and on a refusal too. A
// sample refused for being later than the horizon, or for arriving at a
// full queue, has still been SEEN; re-offering it on every subsequent tick
// would re-refuse it on every subsequent tick and report a storm of
// refusals for one packet. The one outcome that does NOT advance it is
// AidingBeforeHorizon, because that sample was never offered a fusion
// instant and must still be admitted once one exists.
mocapAdmitted_s := if reset then -TimestampMagnitudeLimit
elseif mocapArrival <> AidingNoArrival
and mocapArrival <> AidingBeforeHorizon
then mocap.timestamp_s else pre(mocapAdmitted_s);
mocapHeldRow := if reset then zeros(MocapMeasurementLength)
elseif mocapDelivered then mocapDeliveredRow else pre(mocapHeldRow);
mocapHeldValid := (not reset) and (mocapDelivered or pre(mocapHeldValid));
(mocapOutTimestamp_s,
mocapOutPosition_m,
mocapOutQuaternion,
mocapOutPositionCovariance_m2,
mocapOutAttitudeCovariance_rad2) :=
Estimation.FusionHorizon.unpackMocap(mocapHeldRow);
mocapAtHorizon.timestamp_s := mocapOutTimestamp_s;
mocapAtHorizon.positionWorldEnu_m := mocapOutPosition_m;
mocapAtHorizon.quaternionWorldBody := mocapOutQuaternion;
mocapAtHorizon.positionCovarianceWorld_m2 := mocapOutPositionCovariance_m2;
mocapAtHorizon.attitudeCovarianceBody_rad2 :=
mocapOutAttitudeCovariance_rad2;
mocapAtHorizon.valid := mocapHeldValid;
mocapAtHorizon.fresh := mocapDelivered;
// ---- GPS --------------------------------------------------------------
gpsArrivedRow := Estimation.FusionHorizon.packGps(gps);
gpsArrived := (not reset) and gps.valid
and abs(gps.timestamp_s) < TimestampMagnitudeLimit
and gps.timestamp_s > pre(gpsAdmitted_s) + 1.0e-9;
(gpsStoreSlot,
gpsStoredRow,
gpsHead,
gpsTail,
gpsCount,
gpsDeliveredRow,
gpsAge_s,
gpsDelivered,
gpsArrival,
gpsDelivery) := Estimation.FusionHorizon.stepQueue(
reset, pre(gpsQueue), pre(gpsHead), pre(gpsTail), pre(gpsCount),
gpsArrived, gpsArrivedRow, horizonReleased, horizonValid,
horizonEpoch_s, maximumResidualAge_s, epochTolerance_s);
gpsQueue := Estimation.FusionHorizon.storeMeasurement(
pre(gpsQueue), gpsStoreSlot, gpsStoredRow);
gpsAdmitted_s := if reset then -TimestampMagnitudeLimit
elseif gpsArrival <> AidingNoArrival
and gpsArrival <> AidingBeforeHorizon
then gps.timestamp_s else pre(gpsAdmitted_s);
gpsHeldRow := if reset then zeros(GpsMeasurementLength)
elseif gpsDelivered then gpsDeliveredRow else pre(gpsHeldRow);
gpsHeldValid := (not reset) and (gpsDelivered or pre(gpsHeldValid));
(gpsOutTimestamp_s,
gpsOutPositionValid,
gpsOutVelocityValid,
gpsOutGeodetic_deg_m,
gpsOutPosition_m,
gpsOutVelocity_m_s,
gpsOutPositionCovariance_m2,
gpsOutVelocityCovariance_m2_s2) :=
Estimation.FusionHorizon.unpackGps(gpsHeldRow);
gpsAtHorizon.timestamp_s := gpsOutTimestamp_s;
// A fix never delivered offers neither half of its solution. The stored
// flags say which half the SOLUTION carried; held validity says whether a
// solution has been handed over at all, and both have to hold.
gpsAtHorizon.positionValid := gpsOutPositionValid and gpsHeldValid;
gpsAtHorizon.velocityValid := gpsOutVelocityValid and gpsHeldValid;
gpsAtHorizon.geodetic_deg_m := gpsOutGeodetic_deg_m;
gpsAtHorizon.positionWorldEnu_m := gpsOutPosition_m;
gpsAtHorizon.velocityWorldEnu_m_s := gpsOutVelocity_m_s;
gpsAtHorizon.positionCovarianceWorld_m2 := gpsOutPositionCovariance_m2;
gpsAtHorizon.velocityCovarianceWorld_m2_s2 :=
gpsOutVelocityCovariance_m2_s2;
gpsAtHorizon.valid := gpsHeldValid;
gpsAtHorizon.fresh := gpsDelivered;
// ---- magnetometer -----------------------------------------------------
magnetometerArrivedRow :=
Estimation.FusionHorizon.packMagnetometer(magnetometer);
magnetometerArrived := (not reset) and magnetometer.valid
and abs(magnetometer.timestamp_s) < TimestampMagnitudeLimit
and magnetometer.timestamp_s > pre(magnetometerAdmitted_s) + 1.0e-9;
(magnetometerStoreSlot,
magnetometerStoredRow,
magnetometerHead,
magnetometerTail,
magnetometerCount,
magnetometerDeliveredRow,
magnetometerAge_s,
magnetometerDelivered,
magnetometerArrival,
magnetometerDelivery) := Estimation.FusionHorizon.stepQueue(
reset, pre(magnetometerQueue), pre(magnetometerHead),
pre(magnetometerTail), pre(magnetometerCount),
magnetometerArrived, magnetometerArrivedRow, horizonReleased,
horizonValid, horizonEpoch_s, maximumResidualAge_s, epochTolerance_s);
magnetometerQueue := Estimation.FusionHorizon.storeMeasurement(
pre(magnetometerQueue), magnetometerStoreSlot, magnetometerStoredRow);
magnetometerAdmitted_s := if reset then -TimestampMagnitudeLimit
elseif magnetometerArrival <> AidingNoArrival
and magnetometerArrival <> AidingBeforeHorizon
then magnetometer.timestamp_s else pre(magnetometerAdmitted_s);
magnetometerHeldRow := if reset then zeros(MagnetometerMeasurementLength)
elseif magnetometerDelivered then magnetometerDeliveredRow else pre(magnetometerHeldRow);
magnetometerHeldValid := (not reset) and (magnetometerDelivered or pre(magnetometerHeldValid));
(magnetometerOutTimestamp_s,
magnetometerOutField_T,
magnetometerOutCovariance_T2) :=
Estimation.FusionHorizon.unpackMagnetometer(magnetometerHeldRow);
magnetometerAtHorizon.timestamp_s := magnetometerOutTimestamp_s;
magnetometerAtHorizon.magneticFieldBodyFlu_T := magnetometerOutField_T;
magnetometerAtHorizon.covarianceBody_T2 := magnetometerOutCovariance_T2;
magnetometerAtHorizon.valid := magnetometerHeldValid;
magnetometerAtHorizon.fresh := magnetometerDelivered;
// ---- barometer --------------------------------------------------------
barometerArrivedRow :=
Estimation.FusionHorizon.packBarometer(barometer);
barometerArrived := (not reset) and barometer.valid
and abs(barometer.timestamp_s) < TimestampMagnitudeLimit
and barometer.timestamp_s > pre(barometerAdmitted_s) + 1.0e-9;
(barometerStoreSlot,
barometerStoredRow,
barometerHead,
barometerTail,
barometerCount,
barometerDeliveredRow,
barometerAge_s,
barometerDelivered,
barometerArrival,
barometerDelivery) := Estimation.FusionHorizon.stepQueue(
reset, pre(barometerQueue), pre(barometerHead), pre(barometerTail),
pre(barometerCount), barometerArrived, barometerArrivedRow,
horizonReleased, horizonValid, horizonEpoch_s, maximumResidualAge_s,
epochTolerance_s);
barometerQueue := Estimation.FusionHorizon.storeMeasurement(
pre(barometerQueue), barometerStoreSlot, barometerStoredRow);
barometerAdmitted_s := if reset then -TimestampMagnitudeLimit
elseif barometerArrival <> AidingNoArrival
and barometerArrival <> AidingBeforeHorizon
then barometer.timestamp_s else pre(barometerAdmitted_s);
barometerHeldRow := if reset then zeros(BarometerMeasurementLength)
elseif barometerDelivered then barometerDeliveredRow else pre(barometerHeldRow);
barometerHeldValid := (not reset) and (barometerDelivered or pre(barometerHeldValid));
(barometerOutTimestamp_s,
barometerOutAltitude_m,
barometerOutVariance_m2) :=
Estimation.FusionHorizon.unpackBarometer(barometerHeldRow);
barometerAtHorizon.timestamp_s := barometerOutTimestamp_s;
barometerAtHorizon.altitudeWorldEnu_m := barometerOutAltitude_m;
barometerAtHorizon.variance_m2 := barometerOutVariance_m2;
barometerAtHorizon.valid := barometerHeldValid;
barometerAtHorizon.fresh := barometerDelivered;
// ---- optical flow -----------------------------------------------------
opticalFlowArrivedRow :=
Estimation.FusionHorizon.packOpticalFlow(opticalFlow);
opticalFlowArrived := (not reset) and opticalFlow.valid
and abs(opticalFlow.timestamp_s) < TimestampMagnitudeLimit
and opticalFlow.timestamp_s > pre(opticalFlowAdmitted_s) + 1.0e-9;
(opticalFlowStoreSlot,
opticalFlowStoredRow,
opticalFlowHead,
opticalFlowTail,
opticalFlowCount,
opticalFlowDeliveredRow,
opticalFlowAge_s,
opticalFlowDelivered,
opticalFlowArrival,
opticalFlowDelivery) := Estimation.FusionHorizon.stepQueue(
reset, pre(opticalFlowQueue), pre(opticalFlowHead),
pre(opticalFlowTail), pre(opticalFlowCount),
opticalFlowArrived, opticalFlowArrivedRow, horizonReleased,
horizonValid, horizonEpoch_s, maximumResidualAge_s, epochTolerance_s);
opticalFlowQueue := Estimation.FusionHorizon.storeMeasurement(
pre(opticalFlowQueue), opticalFlowStoreSlot, opticalFlowStoredRow);
opticalFlowAdmitted_s := if reset then -TimestampMagnitudeLimit
elseif opticalFlowArrival <> AidingNoArrival
and opticalFlowArrival <> AidingBeforeHorizon
then opticalFlow.timestamp_s else pre(opticalFlowAdmitted_s);
opticalFlowHeldRow := if reset then zeros(OpticalFlowMeasurementLength)
elseif opticalFlowDelivered then opticalFlowDeliveredRow else pre(opticalFlowHeldRow);
opticalFlowHeldValid := (not reset) and (opticalFlowDelivered or pre(opticalFlowHeldValid));
(opticalFlowOutTimestamp_s,
opticalFlowOutLineOfSight_rad,
opticalFlowOutLineOfSightCovariance_rad2,
opticalFlowOutGyroscope_rad,
opticalFlowOutGyroscopeCovariance_rad2,
opticalFlowOutIntegrationTime_s,
opticalFlowOutGroundDistance_m,
opticalFlowOutGroundDistanceVariance_m2,
opticalFlowOutQuality) :=
Estimation.FusionHorizon.unpackOpticalFlow(opticalFlowHeldRow);
opticalFlowAtHorizon.timestamp_s := opticalFlowOutTimestamp_s;
opticalFlowAtHorizon.integratedLineOfSight_rad :=
opticalFlowOutLineOfSight_rad;
opticalFlowAtHorizon.integratedLineOfSightCovariance_rad2 :=
opticalFlowOutLineOfSightCovariance_rad2;
opticalFlowAtHorizon.integratedGyroscopeBodyFlu_rad :=
opticalFlowOutGyroscope_rad;
opticalFlowAtHorizon.integratedGyroscopeCovariance_rad2 :=
opticalFlowOutGyroscopeCovariance_rad2;
opticalFlowAtHorizon.integrationTime_s := opticalFlowOutIntegrationTime_s;
opticalFlowAtHorizon.groundDistance_m := opticalFlowOutGroundDistance_m;
opticalFlowAtHorizon.groundDistanceVariance_m2 :=
opticalFlowOutGroundDistanceVariance_m2;
opticalFlowAtHorizon.quality := opticalFlowOutQuality;
opticalFlowAtHorizon.valid := opticalFlowHeldValid;
opticalFlowAtHorizon.fresh := opticalFlowDelivered;
// ---- supervision ------------------------------------------------------
// Counts rather than levels, for the reason the accepted-correction count
// exists on the estimator status boundary: a level held for one tick of a
// fast clock is invisible to a slower consumer, and a monotonic count is
// the only signal with a well-defined edge across a rate change.
refusedLateCount := if reset then 0 else pre(refusedLateCount)
+ (if mocapArrival == AidingRefusedLate then 1 else 0)
+ (if gpsArrival == AidingRefusedLate then 1 else 0)
+ (if magnetometerArrival == AidingRefusedLate then 1 else 0)
+ (if barometerArrival == AidingRefusedLate then 1 else 0)
+ (if opticalFlowArrival == AidingRefusedLate then 1 else 0);
refusedOverflowCount := if reset then 0 else pre(refusedOverflowCount)
+ (if mocapArrival == AidingRefusedOverflow then 1 else 0)
+ (if gpsArrival == AidingRefusedOverflow then 1 else 0)
+ (if magnetometerArrival == AidingRefusedOverflow then 1 else 0)
+ (if barometerArrival == AidingRefusedOverflow then 1 else 0)
+ (if opticalFlowArrival == AidingRefusedOverflow then 1 else 0);
droppedStaleCount := if reset then 0 else pre(droppedStaleCount)
+ (if mocapDelivery == AidingDroppedStale then 1 else 0)
+ (if gpsDelivery == AidingDroppedStale then 1 else 0)
+ (if magnetometerDelivery == AidingDroppedStale then 1 else 0)
+ (if barometerDelivery == AidingDroppedStale then 1 else 0)
+ (if opticalFlowDelivery == AidingDroppedStale then 1 else 0);
beforeHorizonCount := if reset then 0 else pre(beforeHorizonCount)
+ (if mocapArrival == AidingBeforeHorizon then 1 else 0)
+ (if gpsArrival == AidingBeforeHorizon then 1 else 0)
+ (if magnetometerArrival == AidingBeforeHorizon then 1 else 0)
+ (if barometerArrival == AidingBeforeHorizon then 1 else 0)
+ (if opticalFlowArrival == AidingBeforeHorizon then 1 else 0);
deliveredCount := if reset then 0 else pre(deliveredCount)
+ (if mocapDelivered then 1 else 0)
+ (if gpsDelivered then 1 else 0)
+ (if magnetometerDelivered then 1 else 0)
+ (if barometerDelivered then 1 else 0)
+ (if opticalFlowDelivered then 1 else 0);
// ORDER AND EPOCH, checked against the ROWS rather than against the
// connectors. Both are invariants of this block, and both are latched: a
// single violation anywhere in a flight has to remain visible, which a
// comparison written against the current tick alone would forget on the
// next one.
deliveryOutOfOrder := (not reset) and (pre(deliveryOutOfOrder)
or (mocapDelivered
and mocapDeliveredRow[1] <= pre(mocapLastDelivered_s))
or (gpsDelivered and gpsDeliveredRow[1] <= pre(gpsLastDelivered_s))
or (magnetometerDelivered
and magnetometerDeliveredRow[1] <= pre(magnetometerLastDelivered_s))
or (barometerDelivered
and barometerDeliveredRow[1] <= pre(barometerLastDelivered_s))
or (opticalFlowDelivered
and opticalFlowDeliveredRow[1] <= pre(opticalFlowLastDelivered_s)));
deliveryAfterHorizon := (not reset) and (pre(deliveryAfterHorizon)
or (mocapDelivered
and mocapDeliveredRow[1] > horizonEpoch_s + epochTolerance_s)
or (gpsDelivered
and gpsDeliveredRow[1] > horizonEpoch_s + epochTolerance_s)
or (magnetometerDelivered
and magnetometerDeliveredRow[1] > horizonEpoch_s + epochTolerance_s)
or (barometerDelivered
and barometerDeliveredRow[1] > horizonEpoch_s + epochTolerance_s)
or (opticalFlowDelivered
and opticalFlowDeliveredRow[1] > horizonEpoch_s + epochTolerance_s));
mocapLastDelivered_s := if reset then -TimestampMagnitudeLimit
elseif mocapDelivered then mocapDeliveredRow[1]
else pre(mocapLastDelivered_s);
gpsLastDelivered_s := if reset then -TimestampMagnitudeLimit
elseif gpsDelivered then gpsDeliveredRow[1]
else pre(gpsLastDelivered_s);
magnetometerLastDelivered_s := if reset then -TimestampMagnitudeLimit
elseif magnetometerDelivered then magnetometerDeliveredRow[1]
else pre(magnetometerLastDelivered_s);
barometerLastDelivered_s := if reset then -TimestampMagnitudeLimit
elseif barometerDelivered then barometerDeliveredRow[1]
else pre(barometerLastDelivered_s);
opticalFlowLastDelivered_s := if reset then -TimestampMagnitudeLimit
elseif opticalFlowDelivered then opticalFlowDeliveredRow[1]
else pre(opticalFlowLastDelivered_s);
aidingRefused := refusedLateCount <> pre(refusedLateCount)
or refusedOverflowCount <> pre(refusedOverflowCount)
or droppedStaleCount <> pre(droppedStaleCount);
// The residual the filter still transports over, published as its running
// worst case. This is the number the delayed-aiding claim is measured on,
// so it is a signal and not a comment.
worstDeliveredAge_s := if reset then 0.0
else max(max(max(max(max(pre(worstDeliveredAge_s), mocapAge_s),
gpsAge_s), magnetometerAge_s), barometerAge_s), opticalFlowAge_s);
end when;
equation
// ---- preconditions, refused rather than rounded --------------------------
// A residual bound tighter than one release period cannot be met by a queue
// that ripens on releases: a measurement stamped just after one release
// waits until the next, so its residual reaches one release period by
// construction and every measurement would be discarded as stale. That is a
// configuration that silently stops all aiding, which is exactly the class
// of failure the horizon exists to make impossible.
assert(maximumResidualAge_s >= fusionPeriod_s,
"maximumResidualAge_s must be at least fusionPeriod_s: a measurement
ripens on the first release at or after its own timestamp, so its
residual reaches one release period by construction and a tighter bound
discards every measurement as stale while reporting a stale drop rather
than a configuration error");
// THE HORIZON MUST COVER THE SLOWEST SOURCE, with headroom. This is the
// relation that makes the older-than-the-horizon refusal an ANOMALY path
// rather than a routine one: inside it, every declared source ripens with
// time to spare and only a genuinely late packet is refused. A horizon
// shorter than the sum turns the refusal into the normal case for the
// slowest source, which is aiding silently switched off and reported as a
// per-measurement timestamp fault.
//
// PX4 ships the same rule for the same reason: EKF2_DELAY_MAX is documented
// as needing to be at least the largest EKF2_XXX_DELAY. This adds the
// jitter headroom and makes it refuse rather than advise.
assert(fusionHorizon_s >= maximumSourceDelay_s + horizonJitterMargin_s,
"fusionHorizon_s must cover the slowest declared aiding source plus the
jitter margin: a horizon shorter than maximumSourceDelay_s +
horizonJitterMargin_s reaches a measurement's timestamp only after it has
passed, so that source is refused as late on every packet and its aiding
is off while the reported fault is per-measurement");
// CAPACITY IS NOT ASSERTED, and the reason is worth stating rather than
// leaving as an omission. An earlier version of this block asserted that
// every queue could hold one horizon of its own source. With the depths
// derived from the periods above that inequality is an identity -- the
// depth IS the ceiling of the ratio, plus slack -- so the assertion could
// not fail for any configuration the block admits. A supervision check that
// cannot fire is worse than none: it reads as coverage and is not.
//
// What actually protects the capacity is that the depth and the rate it is
// derived from cannot be set independently, which is why the depths are
// final. A source that delivers faster than its declared period is the
// remaining case, and it is not a configuration error the block can see at
// translation; it shows up at run time as refusedOverflowCount, which is
// published.
annotation(Documentation(info = "<html>
<p>The half of a delayed-fusion architecture that the merged horizon did
not have. <code>Estimation.FusionHorizon.OutputPredictor</code> moves the
filter's own epoch back to <code>t - D</code>; without this block the
aiding streams still arrive at the live edge, so every measurement is
stamped AHEAD of the instant the filter is standing on and is rejected on
its own timestamp rule. This block holds each measurement until the fusion
instant reaches its timestamp and hands it over then.</p>
<p><b>What this retires.</b> Measurement-age alignment over the whole
transport latency. On the live-edge path a GPS fix about 100 ms old was
fused by retrodicting the nominal state over that age and transporting the
measurement Jacobian back through <code>Phi(-age)</code>, admitted by a
<code>maximumAidingDelay_s</code> of 0.25 s. Here the fix is fused at the
instant it was taken. What REMAINS is sub-window alignment: a measurement
timestamp falls between two fusion instants, so the residual is in
<code>[0, fusionPeriod_s)</code> and the same transport runs over that.
The bound the transport error is charged against therefore falls from
0.25 s to one release period, 0.01 s at the flight lattice, and the
residual is published as <code>worstDeliveredAge_s</code> rather than
argued.</p>
<p><b>What retires by construction rather than by code.</b> The gain and
the Joseph posterior on the live-edge path neglect the process noise
accumulated between a measurement's timestamp and the instant it was fused
at. That window is the residual above, so the neglected term shrinks with
it by the same factor of twenty-five; it is not removed, and this
documentation does not claim it is. The claim it does support is that the
window is now a PARAMETER of the release lattice rather than whatever age
a packet happened to arrive with.</p>
<p><b>Ordering.</b> Each source's queue is a FIFO and delivers in
timestamp order, oldest first. A measurement that arrives out of order
relative to one already admitted from the same source is refused by the
novelty rule, exactly as the filters refuse it today; a measurement that
arrives after the fusion instant has passed it is refused with
<code>AidingRefusedLate</code>. Between sources there is no ordering to
keep: each queue is independent and the filter's own correction dispatch
ranks the sources by authority.</p>
<p><b>One hazard this does not guard, stated rather than left to be
found.</b> A measurement stamped in the FUTURE -- past the fusion instant
by more than the horizon, which no honest sensor produces and a sensor with
a broken clock does -- occupies the head of its queue and cannot ripen
until the fusion instant reaches it. The queue behind it fills, and from
then on every arrival from that source is refused for overflow. The source
is effectively blind until the epoch catches up.</p>
<p>It is not silent: <code>refusedOverflowCount</code> climbs on every
refused arrival, which is the bar every other failure here is held to. It
is also not guarded, and guarding it would mean declaring how far ahead of
the fusion instant a timestamp may plausibly be, which is another parameter
and another negative test. The bound already exists in one direction --
<code>maximumResidualAge_s</code> refuses a measurement the horizon has
passed -- and the symmetric one belongs with it rather than bolted on
here.</p>
<p><b>Nothing here knows what a filter is.</b> No covariance, no error
state, no tangent, no injection: this block moves timestamped records in
time and does nothing else, so it serves an additive-bias ESKF and a
manifold UKF without either being privileged, which is the same property
that makes the delta ring estimator-agnostic.</p>
</html>"));
end AidingBuffer;