function

Estimation.StrapdownINS.UKF.predictPreintegratedNominalVector

Apply a shared IMU preintegral to one UKF sigma-state bias hypothesis

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`previous`	`NominalVector`
input	`imu`	`Avionics.ImuSample`
input	`gravityWorldEnu_m_s2`	`Real`	`[3]`
output	`predicted`	`NominalVector`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function predictPreintegratedNominalVector
  "Apply a shared IMU preintegral to one UKF sigma-state bias hypothesis"
  input NominalVector previous;
  input Avionics.ImuSample imu;
  input Real gravityWorldEnu_m_s2[3];
  output NominalVector predicted;
protected
  Real deltaPositionBodyFlu_m[3];
  Real deltaVelocityBodyFlu_m_s[3];
  Real deltaQuaternionBodyFlu[4];
  Real previousRotationWorldBody[3, 3];
  Real dt;
algorithm
  dt := imu.integrationTime_s;
  (deltaPositionBodyFlu_m,
   deltaVelocityBodyFlu_m_s,
   deltaQuaternionBodyFlu) :=
    Estimation.StrapdownINS.correctPreintegratedImu(
      imu, previous[11:13], previous[14:16]);
  previousRotationWorldBody :=
    LieGroups.SO3.Quat.to_DCM(previous[7:10]);
  predicted := cat(1,
    previous[1:3] + previous[4:6] * dt
      + 0.5 * gravityWorldEnu_m_s2 * dt * dt
      + previousRotationWorldBody * deltaPositionBodyFlu_m,
    previous[4:6] + gravityWorldEnu_m_s2 * dt
      + previousRotationWorldBody * deltaVelocityBodyFlu_m_s,
    LieGroups.SO3.Quat.normalize(
      LieGroups.SO3.Quat.product(
        previous[7:10], deltaQuaternionBodyFlu)),
    previous[11:16]);
end predictPreintegratedNominalVector;