function

Estimation.StrapdownINS.UKF.localError

Body-local tangent carrying reference to target

Public declarations

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

Role	Name	Type
input	`reference`	`Estimation.StrapdownINS.ESKF.NominalState`
input	`target`	`Estimation.StrapdownINS.ESKF.NominalState`
output	`error`	`TangentVector`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function localError
  "Body-local tangent carrying reference to target"
  input Estimation.StrapdownINS.ESKF.NominalState reference;
  input Estimation.StrapdownINS.ESKF.NominalState target;
  output TangentVector error;
protected
  Real referencePose[10];
  Real targetPose[10];
  Real groupError[10];
algorithm
  referencePose := cat(1, reference.positionWorldEnu_m,
    reference.velocityWorldEnu_m_s, reference.quaternionWorldBody);
  targetPose := cat(1, target.positionWorldEnu_m,
    target.velocityWorldEnu_m_s, target.quaternionWorldBody);
  groupError := LieGroups.SE23.Quat.product(
    LieGroups.SE23.Quat.inverse(referencePose), targetPose);
  error := cat(1,
    LieGroups.SE23.Quat.log_map(groupError),
    target.gyroscopeBiasBodyFlu_rad_s
      - reference.gyroscopeBiasBodyFlu_rad_s,
    target.accelerometerBiasBodyFlu_m_s2
      - reference.accelerometerBiasBodyFlu_m_s2);
end localError;