function

Control.Multirotor.LogLinear.stateError

Compute the left-invariant SE_2(3) error log(actual^(-1) reference)

The actual and reference navigation states are represented as SE_2(3) elements {position, velocity, quaternion}. The logarithm of actual^-1 reference expresses the correction in actual-body coordinates.

Public declarations

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

Role	Name	Type	Dimensions / attributes / default	Description
input	`positionWorld`	`Real`	`[3](each unit = "m")`	Actual world position
input	`velocityWorld`	`Real`	`[3](each unit = "m/s")`	Actual world velocity
input	`quaternionWorldBody`	`Real`	`[4](each unit = "1")`	Actual body-to-world quaternion {w,x,y,z}
input	`positionReferenceWorld`	`Real`	`[3](each unit = "m")`	Reference world position
input	`velocityReferenceWorld`	`Real`	`[3](each unit = "m/s")`	Reference world velocity
input	`quaternionReferenceWorldBody`	`Real`	`[4](each unit = "1")`	Reference body-to-world quaternion {w,x,y,z}
output	`error`	`Real`	`[9]`	SE_2(3) tangent ordered {position,velocity,rotation}

Modelica declarations and implementation

Show parameters, interfaces and source

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

function stateError
    "Compute the left-invariant SE_2(3) error log(actual^(-1) reference)"
    input Real positionWorld[3](each unit = "m") "Actual world position";
    input Real velocityWorld[3](each unit = "m/s") "Actual world velocity";
    input Real quaternionWorldBody[4](each unit = "1")
      "Actual body-to-world quaternion {w,x,y,z}";
    input Real positionReferenceWorld[3](each unit = "m")
      "Reference world position";
    input Real velocityReferenceWorld[3](each unit = "m/s")
      "Reference world velocity";
    input Real quaternionReferenceWorldBody[4](each unit = "1")
      "Reference body-to-world quaternion {w,x,y,z}";
    output Real error[9]
      "SE_2(3) tangent ordered {position,velocity,rotation}";
  protected
    Real actual[10];
    Real reference[10];
  algorithm
    actual := cat(1, positionWorld, velocityWorld, quaternionWorldBody);
    reference := cat(
      1,
      positionReferenceWorld,
      velocityReferenceWorld,
      quaternionReferenceWorldBody);
    error := LieGroups.SE23.Quat.log_map(
      LieGroups.SE23.Quat.product(
        LieGroups.SE23.Quat.inverse(actual),
        reference));
    annotation(Documentation(info="<html>
      <p>The actual and reference navigation states are represented as
      SE_2(3) elements <code>{position, velocity, quaternion}</code>. The
      logarithm of <code>actual^-1 reference</code> expresses the correction
      in actual-body coordinates.</p>
    </html>"));
  end stateError;