function

Estimation.StrapdownINS.UKF.localErrorVector

Body-local tangent carrying one flattened nominal state to another

Public declarations

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

Role	Name	Type
input	`reference`	`NominalVector`
input	`target`	`NominalVector`
output	`error`	`TangentVector`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function localErrorVector
  "Body-local tangent carrying one flattened nominal state to another"
  input NominalVector reference;
  input NominalVector target;
  output TangentVector error;
protected
  Real groupError[10];
algorithm
  groupError := LieGroups.SE23.Quat.product(
    LieGroups.SE23.Quat.inverse(reference[1:10]), target[1:10]);
  error := cat(1, LieGroups.SE23.Quat.log_map(groupError),
    target[11:13] - reference[11:13],
    target[14:16] - reference[14:16]);
end localErrorVector;