Estimation.StrapdownINS.UKF.step

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

function step
  "Execute one sampled UKF prediction/correction tick"
  input Boolean initializedPrevious;
  input State previous;
  input Boolean reset;
  input Avionics.ImuSample imu;
  input Avionics.MocapSample mocap;
  input Avionics.GpsSample gps;
  input Avionics.MagnetometerSample magnetometer;
  input Avionics.BarometerSample barometer;
  input Avionics.OpticalFlowSample opticalFlow;
  input Real gravityWorldEnu_m_s2[3];
  input Real dt(unit = "s");
  input Real initialPositionWorldEnu_m[3];
  input Real initialVelocityWorldEnu_m_s[3];
  input Real initialQuaternionWorldBody[4];
  input Real initialGyroscopeBiasBodyFlu_rad_s[3];
  input Real initialAccelerometerBiasBodyFlu_m_s2[3];
  input Estimation.StrapdownINS.InitialVariances initialVariances;
  input Estimation.StrapdownINS.ProcessNoise processNoise;
  input Real innovationGate;
  input Real localMagneticFieldWorldEnu_T[3];
  input Real barometerBias_m;
  input Real barometerBiasVariance_m2;
  input Real maximumAidingDelay_s;
  input Real minimumOpticalFlowQuality;
  input Real minimumOpticalFlowGroundDistance_m;
  input Integer mocapRejectionsPrevious;
  input Integer gpsRejectionsPrevious;
  input Integer opticalFlowRejectionsPrevious;
  input Real imuTimestampConsumedPrevious_s;
  input Real mocapTimestampConsumedPrevious_s;
  input Real gpsTimestampConsumedPrevious_s;
  input Real magnetometerTimestampConsumedPrevious_s;
  input Real barometerTimestampConsumedPrevious_s;
  input Real opticalFlowTimestampConsumedPrevious_s;
  output Real positionWorldEnu_m[3];
  output Real velocityWorldEnu_m_s[3];
  output Real quaternionWorldBody[4];
  output Real gyroscopeBiasBodyFlu_rad_s[3];
  output Real accelerometerBiasBodyFlu_m_s2[3];
  output Real covariance[TangentLength, TangentLength];
  output Boolean initialized;
  output Boolean predictionAccepted;
  output Boolean mocapCorrectionAccepted;
  output Boolean gpsCorrectionAccepted;
  output Boolean magnetometerCorrectionAccepted;
  output Boolean barometerCorrectionAccepted;
  output Boolean opticalFlowCorrectionAccepted;
  output Integer correctionOutcome;
  output Integer correctionSource;
  output Real normalizedInnovationSquared;
  output Integer mocapRejections;
  output Integer gpsRejections;
  output Integer opticalFlowRejections;
  output Real imuTimestampConsumed_s;
  output Real mocapTimestampConsumed_s;
  output Real gpsTimestampConsumed_s;
  output Real magnetometerTimestampConsumed_s;
  output Real barometerTimestampConsumed_s;
  output Real opticalFlowTimestampConsumed_s;
protected
  State working;
  State corrected;
  Boolean correctionAccepted;
  Boolean alignmentAccepted;
  Boolean imuNew;
  Boolean mocapNew;
  Boolean gpsNew;
  Boolean magnetometerNew;
  Boolean barometerNew;
  Boolean opticalFlowNew;
  Boolean imuPayloadFinite;
  Boolean mocapSeedUsable;
  Boolean gpsSeedUsable;
  Boolean magnetometerSeedUsable;
  Real mocapSeedQuaternionNorm;
  Real initializationPosition[3];
  Real initializationQuaternion[4];
algorithm
  predictionAccepted := false;
  correctionAccepted := false;
  correctionOutcome := Estimation.StrapdownINS.CorrectionNotAttempted;
  correctionSource := Estimation.StrapdownINS.SourceNone;
  normalizedInnovationSquared := 0.0;
  magnetometerCorrectionAccepted := false;
  barometerCorrectionAccepted := false;
  imuTimestampConsumed_s := imuTimestampConsumedPrevious_s;
  mocapTimestampConsumed_s := mocapTimestampConsumedPrevious_s;
  gpsTimestampConsumed_s := gpsTimestampConsumedPrevious_s;
  magnetometerTimestampConsumed_s := magnetometerTimestampConsumedPrevious_s;
  barometerTimestampConsumed_s := barometerTimestampConsumedPrevious_s;
  opticalFlowTimestampConsumed_s := opticalFlowTimestampConsumedPrevious_s;
  // AFFIRMATIVE ADMISSION, matching the ESKF. `valid` is a claim by the
  // driver; finiteness is a property of the numbers, and only the second one
  // protects the prediction path, which has no innovation gate in front of it.
  // A sample that fails here is treated exactly as a missing one.
  imuPayloadFinite := Estimation.StrapdownINS.imuSampleFinite(imu);
  imuNew := imu.valid
    and imuPayloadFinite
    and imu.timestamp_s > imuTimestampConsumedPrevious_s + 1.0e-9;
  mocapNew := mocap.valid
    and abs(mocap.timestamp_s) < ESKF.FiniteMagnitudeLimit
    and mocap.timestamp_s > mocapTimestampConsumedPrevious_s + 1.0e-9;
  gpsNew := gps.valid
    and abs(gps.timestamp_s) < ESKF.FiniteMagnitudeLimit
    and gps.timestamp_s > gpsTimestampConsumedPrevious_s + 1.0e-9;
  magnetometerNew := magnetometer.valid
    and abs(magnetometer.timestamp_s) < ESKF.FiniteMagnitudeLimit
    and magnetometer.timestamp_s
      > magnetometerTimestampConsumedPrevious_s + 1.0e-9;
  barometerNew := barometer.valid
    and abs(barometer.timestamp_s) < ESKF.FiniteMagnitudeLimit
    and barometer.timestamp_s > barometerTimestampConsumedPrevious_s + 1.0e-9;
  opticalFlowNew := opticalFlow.valid
    and abs(opticalFlow.timestamp_s) < ESKF.FiniteMagnitudeLimit
    and opticalFlow.timestamp_s
      > opticalFlowTimestampConsumedPrevious_s + 1.0e-9;

  // Seed admission. initialize() normalizes whatever attitude it is handed, so
  // a seed that cannot prove it is normalizable would become a full-scale
  // rotation the filter can never be argued out of.
  mocapSeedUsable := mocap.valid;
  for axis in 1:3 loop
    mocapSeedUsable := mocapSeedUsable
      and abs(mocap.positionWorldEnu_m[axis]) < ESKF.FiniteMagnitudeLimit;
  end for;
  for component in 1:4 loop
    mocapSeedUsable := mocapSeedUsable
      and abs(mocap.quaternionWorldBody[component]) < ESKF.FiniteMagnitudeLimit;
  end for;
  mocapSeedQuaternionNorm := mocap.quaternionWorldBody
    * mocap.quaternionWorldBody;
  mocapSeedUsable := mocapSeedUsable
    and sqrt(mocapSeedQuaternionNorm) >= ESKF.MinimumSeedQuaternionNorm;
  gpsSeedUsable := gps.valid and gps.positionValid;
  for axis in 1:3 loop
    gpsSeedUsable := gpsSeedUsable
      and abs(gps.positionWorldEnu_m[axis]) < ESKF.FiniteMagnitudeLimit;
  end for;
  magnetometerSeedUsable := magnetometer.valid;
  for axis in 1:3 loop
    magnetometerSeedUsable := magnetometerSeedUsable
      and abs(magnetometer.magneticFieldBodyFlu_T[axis])
        < ESKF.FiniteMagnitudeLimit
      and abs(localMagneticFieldWorldEnu_T[axis]) < ESKF.FiniteMagnitudeLimit
      and magnetometer.covarianceBody_T2[axis, axis] > 0.0;
  end for;

  if reset or not initializedPrevious then
    initializationPosition := if gpsSeedUsable then
      gps.positionWorldEnu_m else initialPositionWorldEnu_m;
    if mocapSeedUsable then
      initializationQuaternion := mocap.quaternionWorldBody;
      alignmentAccepted := true;
    elseif imu.valid and imuPayloadFinite and magnetometerSeedUsable then
      (initializationQuaternion, alignmentAccepted) :=
        Estimation.StrapdownINS.initialAlignmentQuaternion(
          imu.specificForceBodyFlu_m_s2,
          magnetometer.magneticFieldBodyFlu_T,
          localMagneticFieldWorldEnu_T,
          initialQuaternionWorldBody);
    else
      initializationQuaternion := initialQuaternionWorldBody;
      alignmentAccepted := false;
    end if;
    working := initialize(
      initializationPosition,
      initialVelocityWorldEnu_m_s,
      initializationQuaternion,
      initialGyroscopeBiasBodyFlu_rad_s,
      initialAccelerometerBiasBodyFlu_m_s2,
      initialVariances);
    initialized := true;
    magnetometerCorrectionAccepted := magnetometerSeedUsable
      and alignmentAccepted
      and not mocapSeedUsable;
  elseif imuNew then
    (working, predictionAccepted) := predictPreintegrated(
      previous, imu, gravityWorldEnu_m_s2, processNoise);
    if not predictionAccepted then
      // A failed sigma-point factorization must not publish a partially
      // constructed prediction. Preserve the last valid navigation state and
      // let common health telemetry expose the rejected prediction.
      working := State(
        positionWorldEnu_m=previous.positionWorldEnu_m,
        velocityWorldEnu_m_s=previous.velocityWorldEnu_m_s,
        quaternionWorldBody=previous.quaternionWorldBody,
        gyroscopeBiasBodyFlu_rad_s=previous.gyroscopeBiasBodyFlu_rad_s,
        accelerometerBiasBodyFlu_m_s2=
          previous.accelerometerBiasBodyFlu_m_s2,
        covariance=previous.covariance);
    end if;
    imuTimestampConsumed_s := imu.timestamp_s;
    initialized := initializedPrevious;
  else
    working := State(
      positionWorldEnu_m=previous.positionWorldEnu_m,
      velocityWorldEnu_m_s=previous.velocityWorldEnu_m_s,
      quaternionWorldBody=previous.quaternionWorldBody,
      gyroscopeBiasBodyFlu_rad_s=previous.gyroscopeBiasBodyFlu_rad_s,
      accelerometerBiasBodyFlu_m_s2=
        previous.accelerometerBiasBodyFlu_m_s2,
      covariance=previous.covariance);
    initialized := initializedPrevious;
  end if;

  if initialized and mocapNew then
    correctionSource := Estimation.StrapdownINS.SourceMocap;
    (corrected, correctionAccepted, correctionOutcome,
      normalizedInnovationSquared) := correctMocap(
        working, mocap, innovationGate);
    working := State(
      positionWorldEnu_m=corrected.positionWorldEnu_m,
      velocityWorldEnu_m_s=corrected.velocityWorldEnu_m_s,
      quaternionWorldBody=corrected.quaternionWorldBody,
      gyroscopeBiasBodyFlu_rad_s=corrected.gyroscopeBiasBodyFlu_rad_s,
      accelerometerBiasBodyFlu_m_s2=
        corrected.accelerometerBiasBodyFlu_m_s2,
      covariance=corrected.covariance);
    mocapTimestampConsumed_s := mocap.timestamp_s;
  elseif initialized and gpsNew
      and gps.positionValid and gps.velocityValid then
    correctionSource := Estimation.StrapdownINS.SourceGps;
    (corrected, correctionAccepted, correctionOutcome,
      normalizedInnovationSquared) := correctGps(
        working, gps, innovationGate,
        imu.timestamp_s - gps.timestamp_s,
        imu.angularVelocityBodyFlu_rad_s,
        imu.specificForceBodyFlu_m_s2, gravityWorldEnu_m_s2,
        maximumAidingDelay_s);
    working := State(
      positionWorldEnu_m=corrected.positionWorldEnu_m,
      velocityWorldEnu_m_s=corrected.velocityWorldEnu_m_s,
      quaternionWorldBody=corrected.quaternionWorldBody,
      gyroscopeBiasBodyFlu_rad_s=corrected.gyroscopeBiasBodyFlu_rad_s,
      accelerometerBiasBodyFlu_m_s2=
        corrected.accelerometerBiasBodyFlu_m_s2,
      covariance=corrected.covariance);
    gpsTimestampConsumed_s := gps.timestamp_s;
  elseif initialized and barometerNew then
    correctionSource := Estimation.StrapdownINS.SourceBarometer;
    (corrected, correctionAccepted, correctionOutcome,
      normalizedInnovationSquared) := correctBarometer(
        working, barometer, barometerBias_m, barometerBiasVariance_m2,
        innovationGate, imu.timestamp_s - barometer.timestamp_s,
        imu.angularVelocityBodyFlu_rad_s,
        imu.specificForceBodyFlu_m_s2, gravityWorldEnu_m_s2,
        maximumAidingDelay_s);
    barometerCorrectionAccepted := correctionAccepted;
    working := State(
      positionWorldEnu_m=corrected.positionWorldEnu_m,
      velocityWorldEnu_m_s=corrected.velocityWorldEnu_m_s,
      quaternionWorldBody=corrected.quaternionWorldBody,
      gyroscopeBiasBodyFlu_rad_s=corrected.gyroscopeBiasBodyFlu_rad_s,
      accelerometerBiasBodyFlu_m_s2=
        corrected.accelerometerBiasBodyFlu_m_s2,
      covariance=corrected.covariance);
    barometerTimestampConsumed_s := barometer.timestamp_s;
  elseif initialized and opticalFlowNew then
    correctionSource := Estimation.StrapdownINS.SourceOpticalFlow;
    (corrected, correctionAccepted, correctionOutcome,
      normalizedInnovationSquared) := correctOpticalFlow(
        working, opticalFlow, innovationGate,
        imu.timestamp_s - opticalFlow.timestamp_s,
        imu.angularVelocityBodyFlu_rad_s,
        imu.specificForceBodyFlu_m_s2, gravityWorldEnu_m_s2,
        maximumAidingDelay_s, minimumOpticalFlowQuality,
        minimumOpticalFlowGroundDistance_m);
    working := State(
      positionWorldEnu_m=corrected.positionWorldEnu_m,
      velocityWorldEnu_m_s=corrected.velocityWorldEnu_m_s,
      quaternionWorldBody=corrected.quaternionWorldBody,
      gyroscopeBiasBodyFlu_rad_s=corrected.gyroscopeBiasBodyFlu_rad_s,
      accelerometerBiasBodyFlu_m_s2=
        corrected.accelerometerBiasBodyFlu_m_s2,
      covariance=corrected.covariance);
    opticalFlowTimestampConsumed_s := opticalFlow.timestamp_s;
  elseif initialized and magnetometerNew then
    correctionSource := Estimation.StrapdownINS.SourceMagnetometer;
    (corrected, correctionAccepted, correctionOutcome,
      normalizedInnovationSquared) := correctMagnetometer(
        working, magnetometer, localMagneticFieldWorldEnu_T,
        innovationGate, imu.timestamp_s - magnetometer.timestamp_s,
        imu.angularVelocityBodyFlu_rad_s,
        imu.specificForceBodyFlu_m_s2, gravityWorldEnu_m_s2,
        maximumAidingDelay_s);
    magnetometerCorrectionAccepted := correctionAccepted;
    working := State(
      positionWorldEnu_m=corrected.positionWorldEnu_m,
      velocityWorldEnu_m_s=corrected.velocityWorldEnu_m_s,
      quaternionWorldBody=corrected.quaternionWorldBody,
      gyroscopeBiasBodyFlu_rad_s=corrected.gyroscopeBiasBodyFlu_rad_s,
      accelerometerBiasBodyFlu_m_s2=
        corrected.accelerometerBiasBodyFlu_m_s2,
      covariance=corrected.covariance);
    magnetometerTimestampConsumed_s := magnetometer.timestamp_s;
  end if;

  mocapCorrectionAccepted := correctionAccepted and
    correctionSource == Estimation.StrapdownINS.SourceMocap;
  gpsCorrectionAccepted := correctionAccepted and
    correctionSource == Estimation.StrapdownINS.SourceGps;
  opticalFlowCorrectionAccepted := correctionAccepted and
    correctionSource == Estimation.StrapdownINS.SourceOpticalFlow;
  mocapRejections := if correctionSource
      == Estimation.StrapdownINS.SourceMocap then
    (if correctionAccepted then 0 else mocapRejectionsPrevious + 1)
    else mocapRejectionsPrevious;
  gpsRejections := if correctionSource
      == Estimation.StrapdownINS.SourceGps then
    (if correctionAccepted then 0 else gpsRejectionsPrevious + 1)
    else gpsRejectionsPrevious;
  opticalFlowRejections := if correctionSource
      == Estimation.StrapdownINS.SourceOpticalFlow then
    (if correctionAccepted then 0 else opticalFlowRejectionsPrevious + 1)
    else opticalFlowRejectionsPrevious;

  positionWorldEnu_m := working.positionWorldEnu_m;
  velocityWorldEnu_m_s := working.velocityWorldEnu_m_s;
  quaternionWorldBody := working.quaternionWorldBody;
  gyroscopeBiasBodyFlu_rad_s := working.gyroscopeBiasBodyFlu_rad_s;
  accelerometerBiasBodyFlu_m_s2 :=
    working.accelerometerBiasBodyFlu_m_s2;
  covariance := working.covariance;
end step;