package
Estimation.FusionHorizon
Estimator-agnostic delayed fusion horizon and SE_2(3) output predictor
Delayed aiding queues, preintegrated IMU increments and current-time SE_2(3) prediction. Start with HorizonEstimator; its replaceable filter implements PartialEstimator.
Aiding packets wait until their capture timestamps are reached by the
delayed filter. The output predictor reapplies buffered increments and
first-order bias-Jacobian corrections to carry that state to the present.
Buffered increments use a fixed bias anchor; corrections beyond the
configured range raise biasMoveExceeded.
Choose the fusion interval as an integral number of IMU intervals and the horizon as an integral number of fusion intervals. The horizon must cover the maximum source delay plus jitter margin. These values size fixed arrays and are set at translation. Supply an appropriately band-limited IMU stream; this package does not perform sensor anti-alias filtering.
Monitor readiness, late/overflow refusals and the delivered residual age. Budget buffer folds using generated-code measurements on the intended target. Host replay timings do not establish target WCET.
Public declarations
Explicit source declarations; dimensions, modifications and defaults are shown as written, without evaluation.
| Role | Name | Type | Dimensions / attributes / default | Description |
|---|---|---|---|---|
| constant | DeltaLength | Integer | = 56 | Flat storage width of one Estimation.FusionHorizon.Delta: 3 position, 3 velocity, 4 quaternion, 1 span, and five 3x3 bias Jacobians stored row-major. |
| constant | TimestampMagnitudeLimit | Real | = 1.0e30 | A timestamp whose magnitude reaches this is not a time. The delayed measurement queues order themselves by timestamp, so a not-a-number or an unbounded one does not merely produce a wrong answer, it destroys the ordering the whole horizon rests on: every comparison against a not-a-number is false, so such a packet is never ripe, never late, and never leaves the queue. It is refused at arrival instead. The same magnitude the filters use, restated here rather than imported, because nothing in this package may depend on a filter. |
| constant | MocapMeasurementLength | Integer | = 26 | Flat storage width of one Avionics.MocapSample as a queue holds it: 1 timestamp, 3 position, 4 quaternion, 9 position covariance, 9 attitude covariance. valid and fresh are NOT stored: a queued packet is by construction one that was valid when it arrived, and both flags are asserted on the tick it is delivered. |
| constant | GpsMeasurementLength | Integer | = 30 | 1 timestamp, positionValid, velocityValid, 3 geodetic, 3 position, 3 velocity, 9 position covariance, 9 velocity covariance. The two per-solution validity flags ARE stored, because unlike valid and fresh they say which half of the fix is usable, and that is a property of the measurement rather than of its delivery. |
| constant | MagnetometerMeasurementLength | Integer | = 13 | 1 timestamp, 3 field, 9 covariance |
| constant | BarometerMeasurementLength | Integer | = 3 | 1 timestamp, 1 altitude, 1 variance |
| constant | OpticalFlowMeasurementLength | Integer | = 23 | 1 timestamp, 2 line of sight, 4 line-of-sight covariance, 3 gyroscope integral, 9 gyroscope covariance, 1 integration time, 1 ground distance, 1 ground-distance variance, 1 quality |
| constant | AidingNoArrival | Integer | = 0 | No novel, valid, finitely stamped sample was presented on this tick |
| constant | AidingQueued | Integer | = 1 | Stored, to be fused when the fusion instant reaches its timestamp |
| constant | AidingDeliveredOnArrival | Integer | = 5 | Delivered directly at an empty queue's current release |
| constant | AidingRefusedLate | Integer | = 2 | Refused at arrival: the fusion instant had already passed this timestamp by more than the residual alignment covers, so there is no fusion instant left to fuse it at. This outcome is what replaces transporting a measurement Jacobian a quarter of a second backwards to meet it |
| constant | AidingRefusedOverflow | Integer | = 3 | Refused at arrival because the queue was full. The queue keeps what it already holds and the NEW measurement is the one that is lost. That is the opposite of the right answer for a live-edge buffer, and the reason it is right here is worth stating because the intuition runs the other way. In a delayed queue the OLDEST entry is the one closest to being fusable: it is the next one the fusion instant will reach. Dropping it to make room for a newer one throws away the entry that was about to be used and replaces it with one that cannot be used for another horizon. Do that on every arrival and a queue too short for its source never ripens ANY entry -- the contents are a sliding window of measurements that are all still in the filter's future -- so the source goes silent for the whole flight while every arrival is dutifully stored. Measured on an oversampled source before this was corrected: zero deliveries, and every assertion about ordering and residuals still passing, because a queue that delivers nothing violates none of them. Refusing the arrival cannot deadlock, because the delivery path always drains: on every release the oldest entry is either delivered or, if the fusion instant has passed it, discarded as stale. One slot therefore frees per release whatever the source does, and the queue degrades to delivering at the release rate rather than to delivering nothing |
| constant | AidingBeforeHorizon | Integer | = 4 | Presented before the first release, so there is no fusion instant for it to name yet. NOT a refusal and not counted as one: the horizon costs one horizon of start-up, during which the filter has had no inertial packet either and could not have fused anything. The sample is left unconsumed, so a source that holds its packet between pulses has it admitted on the first tick after the horizon becomes real |
| constant | AidingNoDelivery | Integer | = 0 | Nothing was ripe on this tick, or this tick released no window |
| constant | AidingDeliveredAtHorizon | Integer | = 1 | The oldest entry's timestamp has been reached by the fusion instant, and the entry was handed to the filter |
| constant | AidingDroppedStale | Integer | = 2 | The oldest queued entry exceeded the residual alignment bound |
Members
| Name | Kind | Description |
|---|---|---|
| Delta | record | One SE_2(3) preintegral right factor with its bias sensitivities |
| Pose | record | Navigation pose carried by the horizon, with no filter content |
| identityDelta | function | The group identity over a zero span |
| packDelta | function | Flatten one delta into a ring row |
| unpackDelta | function | Read one delta back out of a ring row |
| integrateSample | function | Integrate one IMU interval into a delta measured from the identity |
| composeDelta | function | Compose two adjacent SE_2(3) preintegral right factors and their Jacobians |
| rebiasDelta | function | Move a composed delta from its integration anchor to an estimated bias |
| retireDelta | function | Divide the earliest right factor out of a composed window, exactly |
| composeRows | function | Compose two packed right factors and return a packed row |
| retireRows | function | Divide the earliest packed factor out of a packed product |
| composePose | function | Carry a pose forward by one preintegral right factor |
| predictPose | function | Advance a packed pose with a bias-corrected buffered delta |
| foldBuffer | function | Compose a contiguous run of ring entries, then one trailing row, into one right factor |
| readRow | function | Select one ring row without a dynamic index |
| storeRow | function | Write one delta into the ring and return the ring |
| jacobianBlock | function | Read one 3x3 bias Jacobian out of a ring row |
| step | function | One inertial tick of the buffered horizon: integrate, accumulate, release, predict |
| navigationEstimate | function | Publish the predicted pose in the canonical estimator-independent fields |
| readMeasurement | function | Select one queued measurement without a dynamic index |
| storeMeasurement | function | Write one packed sample into a measurement queue and return the queue |
| packMocap | function | Flatten one mocap sample into a queue row |
| unpackMocap | function | Expand one mocap queue row into connector fields |
| packGps | function | Flatten one GPS sample into a queue row |
| unpackGps | function | Expand one GPS queue row into connector fields |
| packMagnetometer | function | Flatten one magnetometer sample into a queue row |
| unpackMagnetometer | function | Expand one magnetometer queue row into connector fields |
| packBarometer | function | Flatten one barometer sample into a queue row |
| unpackBarometer | function | Expand one barometer queue row into connector fields |
| packOpticalFlow | function | Flatten one optical-flow sample into a queue row |
| unpackOpticalFlow | function | Expand one optical-flow queue row into connector fields |
| stepQueue | function | One inertial tick of one aiding source's delayed-measurement FIFO |
| OutputPredictor | block | SE_2(3) delta ring, delayed fusion window, and output predictor |
| AidingBuffer | block | Bounded per-source measurement queues that hand each aiding packet to the filter at the fusion instant its own timestamp names |
| HorizonEstimator | block | One fusion horizon, any strapdown filter, an inertial-rate published state |
Modelica declarations and implementation
Show parameters, interfaces and source
Parameters, inputs, outputs, defaults and equations from the source file.
package FusionHorizon
"Estimator-agnostic delayed fusion horizon and SE_2(3) output predictor"
constant Integer DeltaLength = 56
"Flat storage width of one Estimation.FusionHorizon.Delta:
3 position, 3 velocity, 4 quaternion, 1 span, and five 3x3 bias
Jacobians stored row-major.";
constant Real TimestampMagnitudeLimit = 1.0e30
"A timestamp whose magnitude reaches this is not a time. The delayed
measurement queues order themselves by timestamp, so a not-a-number or an
unbounded one does not merely produce a wrong answer, it destroys the
ordering the whole horizon rests on: every comparison against a
not-a-number is false, so such a packet is never ripe, never late, and
never leaves the queue. It is refused at arrival instead.
The same magnitude the filters use, restated here rather than imported,
because nothing in this package may depend on a filter.";
constant Integer MocapMeasurementLength = 26
"Flat storage width of one Avionics.MocapSample as a queue holds it:
1 timestamp, 3 position, 4 quaternion, 9 position covariance, 9 attitude
covariance. valid and fresh are NOT stored: a queued packet is by
construction one that was valid when it arrived, and both flags are
asserted on the tick it is delivered.";
constant Integer GpsMeasurementLength = 30
"1 timestamp, positionValid, velocityValid, 3 geodetic, 3 position,
3 velocity, 9 position covariance, 9 velocity covariance. The two
per-solution validity flags ARE stored, because unlike valid and fresh
they say which half of the fix is usable, and that is a property of the
measurement rather than of its delivery.";
constant Integer MagnetometerMeasurementLength = 13
"1 timestamp, 3 field, 9 covariance";
constant Integer BarometerMeasurementLength = 3
"1 timestamp, 1 altitude, 1 variance";
constant Integer OpticalFlowMeasurementLength = 23
"1 timestamp, 2 line of sight, 4 line-of-sight covariance, 3 gyroscope
integral, 9 gyroscope covariance, 1 integration time, 1 ground distance,
1 ground-distance variance, 1 quality";
// ---- arrival outcomes, one per aiding source per inertial tick ----------
// Every one of these is NAMED. Inferring what happened to a measurement from
// the absence of a delivery is the predicate exhaustion the estimator status
// boundary already refuses elsewhere: a packet that never arrived and a
// packet refused for being older than the horizon are different failures,
// and a supervisor has to be able to tell them apart.
constant Integer AidingNoArrival = 0
"No novel, valid, finitely stamped sample was presented on this tick";
constant Integer AidingQueued = 1
"Stored, to be fused when the fusion instant reaches its timestamp";
constant Integer AidingDeliveredOnArrival = 5
"Delivered directly at an empty queue's current release";
constant Integer AidingRefusedLate = 2
"Refused at arrival: the fusion instant had already passed this timestamp
by more than the residual alignment covers, so there is no fusion instant
left to fuse it at. This outcome is what replaces transporting a
measurement Jacobian a quarter of a second backwards to meet it";
constant Integer AidingRefusedOverflow = 3
"Refused at arrival because the queue was full. The queue keeps what it
already holds and the NEW measurement is the one that is lost.
That is the opposite of the right answer for a live-edge buffer, and the
reason it is right here is worth stating because the intuition runs the
other way. In a delayed queue the OLDEST entry is the one closest to
being fusable: it is the next one the fusion instant will reach. Dropping
it to make room for a newer one throws away the entry that was about to
be used and replaces it with one that cannot be used for another horizon.
Do that on every arrival and a queue too short for its source never
ripens ANY entry -- the contents are a sliding window of measurements
that are all still in the filter's future -- so the source goes silent
for the whole flight while every arrival is dutifully stored. Measured on
an oversampled source before this was corrected: zero deliveries, and
every assertion about ordering and residuals still passing, because a
queue that delivers nothing violates none of them.
Refusing the arrival cannot deadlock, because the delivery path always
drains: on every release the oldest entry is either delivered or, if the
fusion instant has passed it, discarded as stale. One slot therefore
frees per release whatever the source does, and the queue degrades to
delivering at the release rate rather than to delivering nothing";
constant Integer AidingBeforeHorizon = 4
"Presented before the first release, so there is no fusion instant for it
to name yet. NOT a refusal and not counted as one: the horizon costs one
horizon of start-up, during which the filter has had no inertial packet
either and could not have fused anything. The sample is left unconsumed,
so a source that holds its packet between pulses has it admitted on the
first tick after the horizon becomes real";
// ---- delivery outcomes -------------------------------------------------
constant Integer AidingNoDelivery = 0
"Nothing was ripe on this tick, or this tick released no window";
constant Integer AidingDeliveredAtHorizon = 1
"The oldest entry's timestamp has been reached by the fusion instant, and
the entry was handed to the filter";
constant Integer AidingDroppedStale = 2
"The oldest queued entry exceeded the residual alignment bound";
annotation(Documentation(info = "<html>
<p>Delayed aiding queues, preintegrated IMU increments and current-time
SE_2(3) prediction. Start with
<a href=\"modelica://Estimation.FusionHorizon.HorizonEstimator\">HorizonEstimator</a>;
its replaceable filter implements
<a href=\"modelica://Estimation.StrapdownINS.PartialEstimator\">PartialEstimator</a>.</p>
<p>Aiding packets wait until their capture timestamps are reached by the
delayed filter. The output predictor reapplies buffered increments and
first-order bias-Jacobian corrections to carry that state to the present.
Buffered increments use a fixed bias anchor; corrections beyond the
configured range raise <code>biasMoveExceeded</code>.</p>
<p>Choose the fusion interval as an integral number of IMU intervals and
the horizon as an integral number of fusion intervals. The horizon must
cover the maximum source delay plus jitter margin. These values size fixed
arrays and are set at translation. Supply an appropriately band-limited
IMU stream; this package does not perform sensor anti-alias filtering.</p>
<p>Monitor readiness, late/overflow refusals and the delivered residual age.
Budget buffer folds using generated-code measurements on the intended
target. Host replay timings do not establish target WCET.</p>
</html>"));
end FusionHorizon;