function

Estimation.MocapExternalOdometryErrorState.step

One discrete geometric error-state EKF prediction and optional pose correction

Public declarations

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

Role	Name	Type	Description
input	`previous`	`Estimation.MocapExternalOdometryErrorState.State`
input	`dt`	`Real`	Prediction interval [s]
input	`measurementValid`	`Boolean`
input	`measurement`	`Estimation.MocapExternalOdometryErrorState.PoseMeasurement`
input	`processNoise`	`Estimation.MocapExternalOdometryErrorState.ProcessNoise`
input	`measurementNoise`	`Estimation.MocapExternalOdometryErrorState.PoseMeasurementNoise`
output	`next`	`Estimation.MocapExternalOdometryErrorState.State`
output	`correctionAccepted`	`Boolean`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function step "One discrete geometric error-state EKF prediction and optional pose correction"
  input Estimation.MocapExternalOdometryErrorState.State previous;
  input Real dt "Prediction interval [s]";
  input Boolean measurementValid;
  input Estimation.MocapExternalOdometryErrorState.PoseMeasurement measurement;
  input Estimation.MocapExternalOdometryErrorState.ProcessNoise processNoise;
  input Estimation.MocapExternalOdometryErrorState.PoseMeasurementNoise measurementNoise;
  output Estimation.MocapExternalOdometryErrorState.State next;
  output Boolean correctionAccepted;
protected
  Estimation.MocapExternalOdometryErrorState.State predicted;
algorithm
  predicted := Estimation.MocapExternalOdometryErrorState.predict(
    previous,
    dt,
    processNoise);
  if measurementValid then
    (next, correctionAccepted) :=
      Estimation.MocapExternalOdometryErrorState.correct(
        predicted,
        PoseMeasurement(
          position=measurement.position,
          attitude=measurement.attitude),
        PoseMeasurementNoise(
          attitude=measurementNoise.attitude,
          position=measurementNoise.position));
  else
    next := predicted;
    correctionAccepted := false;
  end if;
end step;