function

LieGroups.SE23.Quat.exp_mixed_right_increment_jacobian

Right-trivialized derivative of exp_mixed with respect to its right increment

Rule for exp_mixed, right-increment slot. The increment is a plain algebra vector and is perturbed additively; the result is read back on the right at X1 = exp_mixed(X0, l, r, B), so this returns J with

log_map(X1^-1 * exp_mixed(X0, l, r + dr, B)) = J*dr + O(|dr|^2)

Closed form, and the only rule of the three in which the initial state survives. The right increment acts by left multiplication, so its rotation perturbation sweeps the whole accumulated 3-by-2 block U = R_0 N_l + P_0 M through a cross product before the result is transported back into the body frame, while its own increment block is carried through the coupling M:

column k of [e_v | e_p] = -(R_0 R_l)^T [U[:,k]]x J_r(omega_r) d(omega_r)
                          + R_1^T (dN_r M)[:,k]
e_omega                   = (R_0 R_l)^T J_r(omega_r) d(omega_r)

The right increment's own block uses the opposite coupling sign, so dN_r is mixed_increment_matrix_jacobian(r, -B), matching the way exp_mixed builds N_r.

J_r here is right_jacobian_exact, and in this rule the choice decides whether the rule passes at all: the Jacobian is multiplied by the accumulated position block U before the result is transported, so any error in it arrives scaled by the lever arm. Measured against a central difference at 0.099 rad, right_jacobian's two-term series gives 6.4e-7 at a 50 m arm and 2.6e-6 at 200 m, where the closed form gives 1.1e-8 and 2.9e-8, the difference floors for those arms.

Public declarations

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

Role	Name	Type	Dimensions / attributes / default	Description
input	`X0`	`Real`	`[10]`	Initial state {p0, v0, q0}
input	`l`	`Real`	`[9]`	Left (body-frame) algebra element
input	`r`	`Real`	`[9]`	Right (world-frame) algebra element
input	`B`	`Real`	`[2, 2]`	Coupling matrix
output	`J`	`Real`	`[9, 9]`	d/d(dr) of log_map(X1^-1 * exp_mixed(X0, l, r + dr, B)), X1 = exp_mixed(X0, l, r, B)

Modelica declarations and implementation

Show parameters, interfaces and source

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

function exp_mixed_right_increment_jacobian
  "Right-trivialized derivative of exp_mixed with respect to its right increment"
  input Real X0[10] "Initial state {p0, v0, q0}";
  input Real l[9] "Left (body-frame) algebra element";
  input Real r[9] "Right (world-frame) algebra element";
  input Real B[2, 2] "Coupling matrix";
  output Real J[9, 9]
    "d/d(dr) of log_map(X1^-1 * exp_mixed(X0, l, r + dr, B)), X1 = exp_mixed(X0, l, r, B)";
protected
  Real Nl[3, 2];
  Real DNr[6, 9];
  Real leftRotation[4];
  Real initialThenLeft[3, 3];
  Real resultRotation[3, 3];
  Real transportToLeft[3, 3];
  Real transportToResult[3, 3];
  Real M[2, 2];
  Real P0[3, 2];
  Real U[3, 2];
  Real rightJacobian[3, 3];
algorithm
  Nl := LieGroups.SE23.Quat.mixed_increment_matrix(l, B);
  DNr := LieGroups.SE23.Quat.mixed_increment_matrix_jacobian(r, -B);
  leftRotation := LieGroups.SO3.Quat.exp_map(l[7:9]);
  initialThenLeft := LieGroups.SO3.Quat.to_DCM(
    LieGroups.SO3.Quat.product(X0[7:10], leftRotation));
  resultRotation := LieGroups.SO3.Quat.to_DCM(
    LieGroups.SO3.Quat.product(
      LieGroups.SO3.Quat.product(
        LieGroups.SO3.Quat.exp_map(r[7:9]), X0[7:10]),
      leftRotation));
  transportToLeft := transpose(initialThenLeft);
  transportToResult := transpose(resultRotation);
  M := identity(2) + B;
  P0 := {{X0[4], X0[1]}, {X0[5], X0[2]}, {X0[6], X0[3]}};
  U := LieGroups.SO3.Quat.to_DCM(X0[7:10]) * Nl + P0 * M;
  rightJacobian := LieGroups.SO3.Quat.right_jacobian_exact(r[7:9]);
  J := zeros(9, 9);
  J[1:3, 7:9] := -transportToLeft * LieGroups.SO3.Quat.wedge(U[:, 2])
    * rightJacobian;
  J[4:6, 7:9] := -transportToLeft * LieGroups.SO3.Quat.wedge(U[:, 1])
    * rightJacobian;
  J[1:3, :] := J[1:3, :] + transportToResult
    * (M[1, 2] * DNr[1:3, :] + M[2, 2] * DNr[4:6, :]);
  J[4:6, :] := J[4:6, :] + transportToResult
    * (M[1, 1] * DNr[1:3, :] + M[2, 1] * DNr[4:6, :]);
  J[7:9, 7:9] := transportToLeft * rightJacobian;
  annotation(Documentation(info="<html>
    <p>Rule for <code>exp_mixed</code>, right-increment slot. The increment is a
    plain algebra vector and is perturbed additively; the result is read back on
    the right at X1 = exp_mixed(X0, l, r, B), so this returns J with</p>
    <pre>log_map(X1^-1 * exp_mixed(X0, l, r + dr, B)) = J*dr + O(|dr|^2)</pre>
    <p>Closed form, and the only rule of the three in which the initial state
    survives. The right increment acts by left multiplication, so its rotation
    perturbation sweeps the whole accumulated 3-by-2 block
    U = R_0 N_l + P_0 M through a cross product before the result is
    transported back into the body frame, while its own increment block is
    carried through the coupling M:</p>
    <pre>column k of [e_v | e_p] = -(R_0 R_l)^T [U[:,k]]x J_r(omega_r) d(omega_r)
                          + R_1^T (dN_r M)[:,k]
e_omega                   = (R_0 R_l)^T J_r(omega_r) d(omega_r)</pre>
    <p>The right increment's own block uses the opposite coupling sign, so dN_r
    is <code>mixed_increment_matrix_jacobian(r, -B)</code>, matching the way
    <code>exp_mixed</code> builds N_r.</p>
    <p>J_r here is <code>right_jacobian_exact</code>, and in this rule the
    choice decides whether the rule passes at all: the Jacobian is multiplied by
    the accumulated position block U before the result is transported, so any
    error in it arrives scaled by the lever arm. Measured against a central
    difference at 0.099 rad, <code>right_jacobian</code>'s two-term series gives
    6.4e-7 at a 50 m arm and 2.6e-6 at 200 m, where the closed form gives 1.1e-8
    and 2.9e-8, the difference floors for those arms.</p>
  </html>"));
end exp_mixed_right_increment_jacobian;