function

Estimation.StrapdownINS.ESKF.predictNominal

Propagate the nominal state with the existing mixed SE_2(3) exponential

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`previous`	`NominalState`
input	`angularVelocityMeasuredBodyFlu_rad_s`	`Real`	`[3]`
input	`specificForceMeasuredBodyFlu_m_s2`	`Real`	`[3]`
input	`gravityWorldEnu_m_s2`	`Real`	`[3]`
input	`dt`	`Real`	`(unit = "s")`
output	`predicted`	`NominalState`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function predictNominal
  "Propagate the nominal state with the existing mixed SE_2(3) exponential"
  input NominalState previous;
  input Real angularVelocityMeasuredBodyFlu_rad_s[3];
  input Real specificForceMeasuredBodyFlu_m_s2[3];
  input Real gravityWorldEnu_m_s2[3];
  input Real dt(unit = "s");
  output NominalState predicted;
protected
  Real correctedAngularVelocity[3];
  Real correctedSpecificForce[3];
  Real previousExtendedPose[10];
  Real leftIncrement[9];
  Real rightIncrement[9];
  Real predictedExtendedPose[10];
algorithm
  correctedAngularVelocity := angularVelocityMeasuredBodyFlu_rad_s
    - previous.gyroscopeBiasBodyFlu_rad_s;
  correctedSpecificForce := specificForceMeasuredBodyFlu_m_s2
    - previous.accelerometerBiasBodyFlu_m_s2;
  previousExtendedPose := cat(1,
    previous.positionWorldEnu_m,
    previous.velocityWorldEnu_m_s,
    previous.quaternionWorldBody);
  leftIncrement := cat(1,
    zeros(3), correctedSpecificForce * dt, correctedAngularVelocity * dt);
  rightIncrement := cat(1,
    zeros(3), gravityWorldEnu_m_s2 * dt, zeros(3));
  predictedExtendedPose := LieGroups.SE23.Quat.exp_mixed(
    previousExtendedPose,
    leftIncrement,
    rightIncrement,
    [0.0, dt; 0.0, 0.0]);
  predicted := NominalState(
    positionWorldEnu_m=predictedExtendedPose[1:3],
    velocityWorldEnu_m_s=predictedExtendedPose[4:6],
    quaternionWorldBody=LieGroups.SO3.Quat.normalize(
      predictedExtendedPose[7:10]),
    gyroscopeBiasBodyFlu_rad_s=previous.gyroscopeBiasBodyFlu_rad_s,
    accelerometerBiasBodyFlu_m_s2=previous.accelerometerBiasBodyFlu_m_s2);
end predictNominal;