Tests.HorizonEstimatorWiring.Harness

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

model Harness "One filter behind the shared horizon"
    // The filter is chosen by modifying this component from a specialising
    // model below, not by a replaceable slot on the harness that forwards into
    // the horizon. Forwarding one replaceable class into another cannot be
    // checked against the constraining type by the compiler this corpus is
    // pinned to, and a redeclaration that names the filter directly is both
    // accepted and easier to read.
    Estimation.FusionHorizon.HorizonEstimator estimator(
      samplePeriod=0.00125,
      fusionPeriod_s=0.01,
      fusionHorizon_s=0.05,
      // The declared source delay has to match the horizon this harness runs,
      // not the flight one the block defaults to. Left at the default, a 50 ms
      // horizon in front of a source declared to arrive 110 ms late is a
      // configuration the block refuses, and this model would carry it
      // unnoticed because it is gated at translation and never simulated.
      maximumSourceDelay_s=0.04,
      horizonJitterMargin_s=0.01,
      mocapPeriod_s=0.01);
  equation
    // A vehicle at rest, level, at the local origin, with motion capture
    // arriving 40 ms late: inside the 50 ms horizon by the declared margin, so
    // it is queued and delivered at the fusion instant its own timestamp
    // names, with a residual under one release window rather than the whole
    // transport latency.
    //
    // It used to be stamped at the horizon exactly, `time - fusionHorizon_s`,
    // with a comment saying its age was therefore zero. That was true and it
    // was the only reason any aiding reached the filter at all: the harness
    // was hand-aligning the measurement because nothing else did. A packet
    // stamped exactly at the horizon now lands in the band where it arrives
    // already ripe and cannot be delivered until the next release, so it would
    // be discarded as stale.
    estimator.reset = false;
    estimator.angularVelocityMeasuredBodyFlu_rad_s = zeros(3);
    estimator.specificForceMeasuredBodyFlu_m_s2 = {0.0, 0.0, 9.81};
    estimator.mocap.valid = true;
    estimator.mocap.fresh = true;
    estimator.mocap.timestamp_s = time - 0.04;
    estimator.mocap.positionWorldEnu_m = zeros(3);
    estimator.mocap.quaternionWorldBody = {1.0, 0.0, 0.0, 0.0};
    estimator.mocap.positionCovarianceWorld_m2 = identity(3) * 1.0e-4;
    estimator.mocap.attitudeCovarianceBody_rad2 = identity(3) * 1.0e-4;
    estimator.gps.valid = false;
    estimator.gps.fresh = false;
    estimator.gps.positionValid = false;
    estimator.gps.velocityValid = false;
    estimator.gps.timestamp_s = time;
    estimator.gps.geodetic_deg_m = zeros(3);
    estimator.gps.positionWorldEnu_m = zeros(3);
    estimator.gps.velocityWorldEnu_m_s = zeros(3);
    estimator.gps.positionCovarianceWorld_m2 = identity(3);
    estimator.gps.velocityCovarianceWorld_m2_s2 = identity(3);
    estimator.magnetometer.valid = false;
    estimator.magnetometer.fresh = false;
    estimator.magnetometer.timestamp_s = time;
    estimator.magnetometer.magneticFieldBodyFlu_T =
      estimator.filter.localMagneticFieldWorldEnu_T;
    estimator.magnetometer.covarianceBody_T2 = identity(3) * 1.0e-12;
    estimator.barometer.valid = false;
    estimator.barometer.fresh = false;
    estimator.barometer.timestamp_s = time;
    estimator.barometer.altitudeWorldEnu_m = 0.0;
    estimator.barometer.variance_m2 = 1.0;
    estimator.opticalFlow.valid = false;
    estimator.opticalFlow.fresh = false;
    estimator.opticalFlow.timestamp_s = time;
    estimator.opticalFlow.integratedLineOfSight_rad = zeros(2);
    estimator.opticalFlow.integratedLineOfSightCovariance_rad2 = identity(2);
    estimator.opticalFlow.integratedGyroscopeBodyFlu_rad = zeros(3);
    estimator.opticalFlow.integratedGyroscopeCovariance_rad2 = identity(3);
    estimator.opticalFlow.integrationTime_s = 0.01;
    estimator.opticalFlow.groundDistance_m = 1.0;
    estimator.opticalFlow.groundDistanceVariance_m2 = 0.01;
    estimator.opticalFlow.quality = 1.0;
  end Harness;