function

Estimation.StrapdownINS.ESKF.heldInputJacobian

Transport one held gyroscope/accelerometer sample into the local error

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`A`	`Real`	`[TangentLength, TangentLength]`
input	`interval_s`	`Real`
output	`transport`	`Real`	`[TangentLength, 6]`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function heldInputJacobian
  "Transport one held gyroscope/accelerometer sample into the local error"
  input Real A[TangentLength, TangentLength];
  input Real interval_s;
  output Real transport[TangentLength, 6];
protected
  Real fullNoiseInput[TangentLength, ProcessNoiseLength];
  Real G[TangentLength, 6];
  Real first[TangentLength, 6];
  Real second[TangentLength, 6];
  Real third[TangentLength, 6];
algorithm
  fullNoiseInput := noiseInputMatrix();
  G := fullNoiseInput[:, 1:6];
  first := interval_s * A * G;
  second := interval_s * A * first;
  third := interval_s * A * second;
  // Integrate the same third-order transition used by discreteTransition.
  // A negative interval transports the uncertainty of a held input backward.
  transport := interval_s * (G + 0.5 * first
    + (1.0 / 6.0) * second + (1.0 / 24.0) * third);
end heldInputJacobian;