Estimation/StrapdownINS/ESKF/Estimator.mo
Download original .moGitHub source
Documentation: Estimation.StrapdownINS.ESKF.Estimator
within Estimation.StrapdownINS.ESKF;
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;