function

SLAM.Inertial.ES15Dynamics.Matrices

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`rotation`	`Real`	`[3,3] = identity(3)`
input	`force`	`Real`	`[3] = {0.0,0.0,9.81}`
input	`omega`	`Real`	`[3] = {0.0,0.0,0.0}`
output	`F`	`Real`	`[15,15]`
output	`G`	`Real`	`[15,12]`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function Matrices
    input Real rotation[3,3] = identity(3);
    input Real force[3] = {0.0,0.0,9.81};
    input Real omega[3] = {0.0,0.0,0.0};
    output Real F[15,15];
    output Real G[15,12];
  protected
    Real forceSkew[3,3];
    Real omegaSkew[3,3];
  algorithm
    forceSkew := [0.0,-force[3],force[2];
                  force[3],0.0,-force[1];
                  -force[2],force[1],0.0];
    omegaSkew := [0.0,-omega[3],omega[2];
                  omega[3],0.0,-omega[1];
                  -omega[2],omega[1],0.0];
    F := fill(0.0,15,15);
    G := fill(0.0,15,12);
    F[1:3,4:6] := identity(3);
    // Keep the original three-term contraction order.
    for i in 1:3 loop
      for j in 1:3 loop
        F[i+3,j+6] := -(rotation[i,1]*forceSkew[1,j]
                         +rotation[i,2]*forceSkew[2,j]
                         +rotation[i,3]*forceSkew[3,j]);
      end for;
    end for;
    F[4:6,10:12] := -rotation;
    F[7:9,7:9] := -omegaSkew;
    F[7:9,13:15] := -identity(3);
    G[4:6,1:3] := -rotation;
    G[7:9,4:6] := -identity(3);
    G[10:12,7:9] := identity(3);
    G[13:15,10:12] := identity(3);
  end Matrices;