model

SLAM.Inertial.ES15Dynamics

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
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]`

Members

Name	Kind	Description
Matrices	function

Modelica declarations and implementation

Show parameters, interfaces and source

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

model ES15Dynamics
  pure 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;

  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];
algorithm
  (F,G) := Matrices(rotation,force,omega);
end ES15Dynamics;