function

Estimation.StrapdownINS.UKF.injectVector

Right-inject a local tangent into a flattened nominal state

Public declarations

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

Role	Name	Type
input	`nominal`	`NominalVector`
input	`correction`	`TangentVector`
output	`corrected`	`NominalVector`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function injectVector
  "Right-inject a local tangent into a flattened nominal state"
  input NominalVector nominal;
  input TangentVector correction;
  output NominalVector corrected;
protected
  Real correctedExtendedPose[10];
algorithm
  correctedExtendedPose := LieGroups.SE23.Quat.product(
    nominal[1:10], LieGroups.SE23.Quat.exp_map(correction[1:9]));
  corrected := cat(1,
    correctedExtendedPose[1:6],
    LieGroups.SO3.Quat.normalize(correctedExtendedPose[7:10]),
    nominal[11:13] + correction[10:12],
    nominal[14:16] + correction[13:15]);
end injectVector;