function

Estimation.FusionHorizon.predictPose

Advance a packed pose with a bias-corrected buffered delta

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`poseVector`	`Real`	`[10]`
input	`deltaRow`	`Real`	`[DeltaLength]`
input	`gyroscopeBiasMove_rad_s`	`Real`	`[3]`
input	`accelerometerBiasMove_m_s2`	`Real`	`[3]`
input	`gravityWorldEnu_m_s2`	`Real`	`[3]`
input	`correctBias`	`Boolean`	`= true`
output	`predictedVector`	`Real`	`[10]`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function predictPose
  "Advance a packed pose with a bias-corrected buffered delta"
  input Real poseVector[10];
  input Real deltaRow[DeltaLength];
  input Real gyroscopeBiasMove_rad_s[3];
  input Real accelerometerBiasMove_m_s2[3];
  input Real gravityWorldEnu_m_s2[3];
  input Boolean correctBias = true;
  output Real predictedVector[10];
protected
  Estimation.FusionHorizon.Delta delta;
  Estimation.FusionHorizon.Delta corrected;
  Estimation.FusionHorizon.Pose pose;
  Estimation.FusionHorizon.Pose predicted;
algorithm
  delta := Estimation.FusionHorizon.unpackDelta(deltaRow);
  corrected := if correctBias then Estimation.FusionHorizon.rebiasDelta(
    delta, gyroscopeBiasMove_rad_s, accelerometerBiasMove_m_s2) else delta;
  pose := Estimation.FusionHorizon.Pose(
    positionWorldEnu_m=poseVector[1:3],
    velocityWorldEnu_m_s=poseVector[4:6],
    quaternionWorldBody=poseVector[7:10]);
  predicted := Estimation.FusionHorizon.composePose(pose, corrected, gravityWorldEnu_m_s2);
  predictedVector := cat(1, predicted.positionWorldEnu_m,
    predicted.velocityWorldEnu_m_s, predicted.quaternionWorldBody);
  annotation(Inline = false);
end predictPose;