function

Estimation.MocapExternalOdometryErrorState.inject

Inject a tangent correction into the nominal state

Public declarations

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

Role	Name	Type	Description
input	`state`	`Estimation.MocapExternalOdometryErrorState.NominalState`
input	`correction`	`TangentVector`	{attitude error, velocity error, position error, angular velocity error}
output	`corrected`	`Estimation.MocapExternalOdometryErrorState.NominalState`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function inject "Inject a tangent correction into the nominal state"
  input Estimation.MocapExternalOdometryErrorState.NominalState state;
  input TangentVector correction
    "{attitude error, velocity error, position error, angular velocity error}";
  output Estimation.MocapExternalOdometryErrorState.NominalState corrected;
protected
  Real attitudeDelta[4];
algorithm
  attitudeDelta := LieGroups.SO3.Quat.exp_map(correction[1:3]);
  corrected.attitude := LieGroups.SO3.Quat.normalize(
    LieGroups.SO3.Quat.product(state.attitude, attitudeDelta));
  corrected.velocity := state.velocity + correction[4:6];
  corrected.position := state.position + correction[7:9];
  corrected.angularVelocity := state.angularVelocity + correction[10:12];
end inject;