model

SLAM.Examples.RuntimeEvaluation

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`worldPosition`	`Real`	`[3] = {0.0,0.0,1.5}`
input	`worldQuaternion`	`Real`	`[4] = {1.0,0.0,0.0,0.0}`
input	`originPosition`	`Real`	`[3] = {0.0,0.0,1.5}`
input	`originQuaternion`	`Real`	`[4] = {1.0,0.0,0.0,0.0}`
input	`estimatePosition`	`Real`	`[3] = {0.0,0.0,0.0}`
input	`estimateQuaternion`	`Real`	`[4] = {1.0,0.0,0.0,0.0}`
input	`previousSquaredError`	`Real`	`= 0.0`
input	`sampleCount`	`Real`	`= 1.0`
output	`referencePosition`	`Real`	`[3]`
output	`referenceQuaternion`	`Real`	`[4]`
output	`nextSquaredError`	`Real`
output	`currentError`	`Real`
output	`ate`	`Real`
output	`orientationError`	`Real`

Modelica declarations and implementation

Show parameters, interfaces and source

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

model RuntimeEvaluation
  input Real worldPosition[3] = {0.0,0.0,1.5};
  input Real worldQuaternion[4] = {1.0,0.0,0.0,0.0};
  input Real originPosition[3] = {0.0,0.0,1.5};
  input Real originQuaternion[4] = {1.0,0.0,0.0,0.0};
  input Real estimatePosition[3] = {0.0,0.0,0.0};
  input Real estimateQuaternion[4] = {1.0,0.0,0.0,0.0};
  input Real previousSquaredError = 0.0;
  input Real sampleCount = 1.0;
  output Real referencePosition[3];
  output Real referenceQuaternion[4];
  output Real nextSquaredError;
  output Real currentError;
  output Real ate;
  output Real orientationError;
protected
  Real originYaw; Real c; Real s; Real hc; Real hs;
  Real squaredDistance; Real quaternionDot;
equation
  originYaw = atan2(2.0*(originQuaternion[1]*originQuaternion[4]+originQuaternion[2]*originQuaternion[3]),
    originQuaternion[1]^2+originQuaternion[2]^2-originQuaternion[3]^2-originQuaternion[4]^2);
  c = cos(originYaw); s = sin(originYaw);
  hc = cos(originYaw/2.0); hs = sin(originYaw/2.0);
  referencePosition[1] = c*(worldPosition[1]-originPosition[1])+s*(worldPosition[2]-originPosition[2]);
  referencePosition[2] = -s*(worldPosition[1]-originPosition[1])+c*(worldPosition[2]-originPosition[2]);
  referencePosition[3] = worldPosition[3]-originPosition[3];
  referenceQuaternion = {hc*worldQuaternion[1]+hs*worldQuaternion[4],
    hc*worldQuaternion[2]+hs*worldQuaternion[3],hc*worldQuaternion[3]-hs*worldQuaternion[2],
    hc*worldQuaternion[4]-hs*worldQuaternion[1]};
  squaredDistance = sum((estimatePosition[i]-referencePosition[i])^2 for i in 1:3);
  currentError = sqrt(squaredDistance);
  nextSquaredError = previousSquaredError+squaredDistance;
  ate = sqrt(nextSquaredError/sampleCount);
  quaternionDot = abs(sum(estimateQuaternion[i]*referenceQuaternion[i] for i in 1:4));
  orientationError = 2.0*acos(min(1.0,quaternionDot))*180.0/3.141592653589793;
end RuntimeEvaluation;