function

Estimation.StrapdownINS.ESKF.TickUpdates.FallbackHoldTick

Public declarations

Explicit source declarations; dimensions, modifications and defaults are shown as written, without evaluation.

Role	Name	Type	Dimensions / attributes / default
input	`previous`	`State`
input	`tuning`	`Tuning`
input	`requested`	`Boolean`
input	`imuQuiet`	`Boolean`
input	`positionHold`	`Real`	`[3]`
output	`next`	`State`
output	`attempted`	`Boolean`
output	`zeroVelocityAccepted`	`Boolean`
output	`pseudoPositionAccepted`	`Boolean`
output	`outcome`	`Integer`
output	`source`	`Integer`
output	`nis`	`Real`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function FallbackHoldTick
    input State previous;
    input Tuning tuning;
    input Boolean requested;
    input Boolean imuQuiet;
    input Real positionHold[3];
    output State next;
    output Boolean attempted;
    output Boolean zeroVelocityAccepted;
    output Boolean pseudoPositionAccepted;
    output Integer outcome;
    output Integer source;
    output Real nis;
  protected
    Boolean zeroConfigured; Boolean positionConfigured;
  algorithm
    zeroConfigured := tuning.zeroVelocityVariance_m2_s2 > 0.0
      and tuning.zeroVelocityVariance_m2_s2 < FiniteMagnitudeLimit;
    positionConfigured := tuning.pseudoPositionVariance_m2 > 0.0
      and tuning.pseudoPositionVariance_m2 < FiniteMagnitudeLimit;
    attempted := requested and ((zeroConfigured and imuQuiet) or positionConfigured);
    zeroVelocityAccepted := false; pseudoPositionAccepted := false;
    outcome := CorrectionNotAttempted; source := SourceNone; nis := 0.0;
    if requested and zeroConfigured and imuQuiet then
      (next, zeroVelocityAccepted, outcome, nis) := correctZeroVelocity(previous,
        tuning.zeroVelocityVariance_m2_s2, 0.0, tuning.useSemiDirectBias);
      source := SourceZeroVelocity;
    elseif requested and positionConfigured then
      (next, pseudoPositionAccepted, outcome, nis) := correctPseudoPosition(previous,
        positionHold, tuning.pseudoPositionVariance_m2, 0.0, tuning.useSemiDirectBias);
      source := SourcePseudoPosition;
    else
      next := copyState(previous);
    end if;
  end FallbackHoldTick;