function

LieGroups.SO3.Mrp.from_Quat

Convert quaternion to MRP

Public declarations

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

Role	Name	Type	Dimensions / attributes / default	Description
input	`q`	`Real`	`[4]`	Quaternion {w,x,y,z}
output	`r`	`Real`	`[3]`	MRP parameters

Modelica declarations and implementation

Show parameters, interfaces and source

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

function from_Quat "Convert quaternion to MRP"
  input Real q[4] "Quaternion {w,x,y,z}";
  output Real r[3] "MRP parameters";
protected
  Real q_n[4];
  Real den;
  constant Real eps = 1e-10;
algorithm
  // Ensure positive scalar part
  if q[1] < 0 then
    q_n := -q;
  else
    q_n := q;
  end if;

  den := max(1.0 + q_n[1], eps);
  r[1] := q_n[2] / den;
  r[2] := q_n[3] / den;
  r[3] := q_n[4] / den;
end from_Quat;