CogniPilot

MODELICA LIBRARY · ENGINEERING REFERENCE

SLAM/PoseGraph/PGMultiply.mo

Download original .moGitHub source

Documentation: SLAM.PoseGraph.PGMultiply

within SLAM.PoseGraph;
function PGMultiply
  input Real A[3,3]; input Real B[3,3]; output Real C[3,3];
algorithm
  C[1,1] := A[1,1]*B[1,1]+A[1,2]*B[2,1]+A[1,3]*B[3,1];
  C[1,2] := A[1,1]*B[1,2]+A[1,2]*B[2,2]+A[1,3]*B[3,2];
  C[1,3] := A[1,1]*B[1,3]+A[1,2]*B[2,3]+A[1,3]*B[3,3];
  C[2,1] := A[2,1]*B[1,1]+A[2,2]*B[2,1]+A[2,3]*B[3,1];
  C[2,2] := A[2,1]*B[1,2]+A[2,2]*B[2,2]+A[2,3]*B[3,2];
  C[2,3] := A[2,1]*B[1,3]+A[2,2]*B[2,3]+A[2,3]*B[3,3];
  C[3,1] := A[3,1]*B[1,1]+A[3,2]*B[2,1]+A[3,3]*B[3,1];
  C[3,2] := A[3,1]*B[1,2]+A[3,2]*B[2,2]+A[3,3]*B[3,2];
  C[3,3] := A[3,1]*B[1,3]+A[3,2]*B[2,3]+A[3,3]*B[3,3];
end PGMultiply;