Tests.HorizonBiasSupervisionTests

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

model HorizonBiasSupervisionTests
  "The bias move is bounded and reported, and the drift the incremental path
   does not carry is bounded by a forced re-base"

  constant Real samplePeriod = 0.00125 "800 Hz inertial tick";
  constant Real fusionPeriod_s = 0.01 "100 Hz fusion release";
  constant Real fusionHorizon_s = 0.05 "Five buffered release windows";
  constant Real gravityWorldEnu_m_s2[3] = {0.0, 0.0, -9.81};
  constant Real maximumGyroscopeBiasMove_rad_s = 0.05;
  constant Real maximumPredictorDivergence_rad = 1.0e-3
    "Tighter than the block default on purpose. The default is what the flight
     fold budget leaves, and at the default this model would have to run for
     more than a second per re-anchor to see one. The budget parameters below
     are widened to match, which is legal and is what makes the run short: this
     model measures the re-anchor TRIGGER, and has no way to measure a budget
     that is a property of the generated code on a target it does not run on.";
  constant Real foldBudget_hz = 60.0
    "Wide enough to admit the tolerance above; see the note on it";
  constant Real correctionRateBudget_hz = 0.0;
  constant Real insideBias_rad_s[3] = {0.02, 0.0, 0.0}
    "Inside the declared ball around the anchor, so the move is applied and
     nothing is flagged. 0.02 rad/s against a 1e-3 rad divergence tolerance is
     a forced re-base every 50 ms, which is 40 inertial ticks.";
  constant Real outsideBias_rad_s[3] = {0.1, 0.0, 0.0}
    "Outside it, so biasMoveExceeded stands and the state is published anyway";
  constant Integer reanchorTicks = 40
    "maximumPredictorDivergence_rad / (||db_g|| * samplePeriod) for the inside
     case, derived rather than observed";
  constant Real settled_s = 0.08 "Past the horizon fill and the first release";

  Real elapsed_s(start = 0.0, fixed = true)
    "Continuous anchor. A model assembled only from clocked blocks has no
     continuous equation at all and OpenModelica index reduction refuses to
     build one. Not under test.";

  Estimation.FusionHorizon.OutputPredictor inside(
    samplePeriod=samplePeriod,
    fusionPeriod_s=fusionPeriod_s,
    fusionHorizon_s=fusionHorizon_s,
    gravityWorldEnu_m_s2=gravityWorldEnu_m_s2,
    maximumGyroscopeBiasMove_rad_s=maximumGyroscopeBiasMove_rad_s,
    maximumPredictorDivergence_rad=maximumPredictorDivergence_rad,
    foldBudget_hz=foldBudget_hz,
    correctionRateBudget_hz=correctionRateBudget_hz);
  Estimation.FusionHorizon.OutputPredictor outside(
    samplePeriod=samplePeriod,
    fusionPeriod_s=fusionPeriod_s,
    fusionHorizon_s=fusionHorizon_s,
    gravityWorldEnu_m_s2=gravityWorldEnu_m_s2,
    maximumGyroscopeBiasMove_rad_s=maximumGyroscopeBiasMove_rad_s,
    maximumPredictorDivergence_rad=maximumPredictorDivergence_rad,
    foldBudget_hz=foldBudget_hz,
    correctionRateBudget_hz=correctionRateBudget_hz);

  discrete Integer insideTicksSinceRebase(start = 0, fixed = true);
  discrete Integer worstTicksSinceRebase(start = 0, fixed = true);

algorithm
  when sample(0.0, samplePeriod) then
    insideTicksSinceRebase := if inside.rebased then 0
      else pre(insideTicksSinceRebase) + 1;
    worstTicksSinceRebase := if time < settled_s then 0
      else max(pre(worstTicksSinceRebase), insideTicksSinceRebase);
  end when;

equation
  der(elapsed_s) = 1.0;

  // A level, at-rest stream. The property under test is a supervision
  // property, not a tracking one: the horizon pose handed back below is a
  // fixed stand-in rather than a filter's answer, so the predicted state jumps
  // on every forced re-base and that is expected. What is asserted is the
  // FLAG and the CADENCE, and neither depends on the pose being truthful.
  inside.reset = false;
  inside.angularVelocityMeasuredBodyFlu_rad_s = zeros(3);
  inside.specificForceMeasuredBodyFlu_m_s2 = {0.0, 0.0, 9.81};
  inside.horizonStateValid = inside.horizonReady;
  inside.horizonStateShifted = false;
  inside.horizonPositionWorldEnu_m = zeros(3);
  inside.horizonVelocityWorldEnu_m_s = zeros(3);
  inside.horizonQuaternionWorldBody = {1.0, 0.0, 0.0, 0.0};
  inside.horizonGyroscopeBiasBodyFlu_rad_s = insideBias_rad_s;
  inside.horizonAccelerometerBiasBodyFlu_m_s2 = zeros(3);

  outside.reset = false;
  outside.angularVelocityMeasuredBodyFlu_rad_s = zeros(3);
  outside.specificForceMeasuredBodyFlu_m_s2 = {0.0, 0.0, 9.81};
  outside.horizonStateValid = outside.horizonReady;
  outside.horizonStateShifted = false;
  outside.horizonPositionWorldEnu_m = zeros(3);
  outside.horizonVelocityWorldEnu_m_s = zeros(3);
  outside.horizonQuaternionWorldBody = {1.0, 0.0, 0.0, 0.0};
  outside.horizonGyroscopeBiasBodyFlu_rad_s = outsideBias_rad_s;
  outside.horizonAccelerometerBiasBodyFlu_m_s2 = zeros(3);

  // ---- the bound is reported, not enforced --------------------------------
  // The first-order move is good over a stated ball and no further. Outside
  // it the answer is still the best available one, so it is published and
  // flagged; clamping it would put the predictor on a bias nobody estimated
  // and report nothing.
  assert(outside.biasMoveExceeded or time < settled_s,
    "A bias move twice the declared bound was applied without raising
     biasMoveExceeded, so the first-order validity limit is unobservable");
  assert(not inside.biasMoveExceeded,
    "A bias move inside the declared bound raised biasMoveExceeded");

  // ---- the incremental path's drift is bounded ----------------------------
  // Only a re-base carries the bias move onto the buffered window; between
  // re-bases the predictor leaves the filter's own bias at ||db_g||, without
  // bound in the time since the last one. The re-anchor makes that time
  // bounded by construction, and this is the assertion that says so. Reverting
  // it leaves the counter climbing for the whole run.
  // Measured: 41 ticks, against the derived 40.
  assert(worstTicksSinceRebase <= reanchorTicks + 2,
    "The predictor went longer without a re-base than the declared divergence
     tolerance allows, so the drift the incremental path does not carry is
     unbounded in flight time");
  assert(worstTicksSinceRebase >= reanchorTicks - 2 or time < 0.2,
    "The predictor re-based more often than the divergence tolerance asks for,
     so the trigger is not the one this test is measuring");

  annotation(experiment(StartTime=0.0, StopTime=0.3,
    Tolerance=1.0e-8, Interval=0.001),
    Documentation(info="<html>
    <p>Simulated as a top-level model through
    <code>Tests/run-horizon.mos</code>. Two supervision properties that have no
    other home: the bias-move validity bound is REPORTED rather than clamped,
    and the drift the incremental path does not carry is bounded by a forced
    re-base rather than left to grow with flight time.</p>
    </html>"));
end HorizonBiasSupervisionTests;