function

LieGroups.SE23.Quat.mixed_increment_matrix

Increment block N of the closed-form mixed exponential

N(a, B) = A + A B/2 + Om A (C1 I + C2 B) + Om^2 A (C2 I + C3 B), with A = [a_b | v_b] the 3-by-2 acceleration/velocity block of the algebra element and Om the skew matrix of its rotation part. This is the increment block of the closed-form mixed exponential of Lin, Pant, Perseghetti, and Goppert, IEEE L-CSS 2025.

exp_mixed forms the left increment as N(l, B) and the right increment as N(r, -B): the sign of the coupling is the only difference between them. The derivative rules for exp_mixed are written in terms of this block and of mixed_increment_matrix_jacobian.

Pinned against the primitive by evaluating exp_mixed at the identity element with a zero right increment, where the group product reduces to N(l, B) exactly.

Public declarations

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

Role	Name	Type	Dimensions / attributes / default	Description
input	`a`	`Real`	`[9]`	Algebra element {vb, ab, omega}
input	`B`	`Real`	`[2, 2]`	Coupling matrix as this increment sees it
output	`N`	`Real`	`[3, 2]`	Increment block; column 1 is the velocity slot, column 2 the position slot

Modelica declarations and implementation

Show parameters, interfaces and source

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

function mixed_increment_matrix
  "Increment block N of the closed-form mixed exponential"
  input Real a[9] "Algebra element {vb, ab, omega}";
  input Real B[2, 2] "Coupling matrix as this increment sees it";
  output Real N[3, 2] "Increment block; column 1 is the velocity slot, column 2 the position slot";
protected
  Real omega[3];
  Real theta_sq;
  Real C[3];
  Real Om[3, 3];
  Real Om2[3, 3];
  Real A[3, 2];
  Real I2[2, 2];
algorithm
  omega := a[7:9];
  theta_sq := omega * omega;
  C := LieGroups.SE23.Quat.mixed_increment_coefficients(theta_sq);
  Om := LieGroups.SO3.Quat.wedge(omega);
  Om2 := Om * Om;
  A := transpose({a[4:6], a[1:3]});
  I2 := identity(2);
  N := A + 0.5 * A * B
    + Om * A * (C[1] * I2 + C[2] * B)
    + Om2 * A * (C[2] * I2 + C[3] * B);
  annotation(Documentation(info="<html>
    <p>N(a, B) = A + A B/2 + Om A (C1 I + C2 B) + Om^2 A (C2 I + C3 B), with
    A = [a_b | v_b] the 3-by-2 acceleration/velocity block of the algebra
    element and Om the skew matrix of its rotation part. This is the increment
    block of the closed-form mixed exponential of Lin, Pant, Perseghetti, and
    Goppert, IEEE L-CSS 2025.</p>
    <p><code>exp_mixed</code> forms the left increment as N(l, B) and the right
    increment as N(r, -B): the sign of the coupling is the only difference
    between them. The derivative rules for <code>exp_mixed</code> are written in
    terms of this block and of
    <code>mixed_increment_matrix_jacobian</code>.</p>
    <p>Pinned against the primitive by evaluating
    <code>exp_mixed</code> at the identity element with a zero right increment,
    where the group product reduces to N(l, B) exactly.</p>
  </html>"));
end mixed_increment_matrix;