function

Control.Multirotor.LogLinear.attitudeControl

Compute body angular velocity from an SO(3) logarithmic attitude error

Uses the shortest-path SO(3) logarithm and maps the proportional correction through the left Jacobian. Inputs must be unit, scalar-first Hamilton quaternions.

Public declarations

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

Role	Name	Type	Dimensions / attributes / default	Description
input	`gain`	`Real`	`[3](each unit = "1/s")`	Principal-axis attitude gains
input	`quaternionWorldBody`	`Real`	`[4](each unit = "1")`	Actual body-to-world quaternion
input	`quaternionReferenceWorldBody`	`Real`	`[4](each unit = "1")`	Reference body-to-world quaternion
output	`angularVelocityBody`	`Real`	`[3](each unit = "rad/s")`	Commanded body angular velocity

Modelica declarations and implementation

Show parameters, interfaces and source

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

function attitudeControl
    "Compute body angular velocity from an SO(3) logarithmic attitude error"
    input Real gain[3](each unit = "1/s") "Principal-axis attitude gains";
    input Real quaternionWorldBody[4](each unit = "1")
      "Actual body-to-world quaternion";
    input Real quaternionReferenceWorldBody[4](each unit = "1")
      "Reference body-to-world quaternion";
    output Real angularVelocityBody[3](each unit = "rad/s")
      "Commanded body angular velocity";
  protected
    Real error[3](each unit = "rad") "SO(3) logarithmic error";
  algorithm
    error := LieGroups.SO3.Quat.log_map(
      LieGroups.SO3.Quat.product(
        LieGroups.SO3.Quat.inverse(quaternionWorldBody),
        quaternionReferenceWorldBody));
    angularVelocityBody := LieGroups.SO3.Quat.left_jacobian(error)
      * diagonal(gain) * error;
    annotation(Documentation(info="<html>
      <p>Uses the shortest-path SO(3) logarithm and maps the proportional
      correction through the left Jacobian. Inputs must be unit,
      scalar-first Hamilton quaternions.</p>
    </html>"));
  end attitudeControl;