Tests/EstimationTests.mo

Download original .moGitHub source

Documentation: Tests.EstimationTests · Tests.EstimationTests.run

within Tests;
model EstimationTests "Initialization, prediction, correction, and failure invariants"
  function run "Execute the static estimator assertions"
    output Boolean passed;
  protected
    constant Real tolerance = 1.0e-9;
    Estimation.MocapExternalOdometryErrorState.InitialVariances initialVariances;
    Estimation.MocapExternalOdometryErrorState.InitialVariances zeroVariances;
    Estimation.MocapExternalOdometryErrorState.ProcessNoise noProcessNoise;
    Estimation.MocapExternalOdometryErrorState.ProcessNoise processNoise;
    Estimation.MocapExternalOdometryErrorState.PoseMeasurement exactMeasurement;
    Estimation.MocapExternalOdometryErrorState.PoseMeasurement negativeQuaternionMeasurement;
    Estimation.MocapExternalOdometryErrorState.PoseMeasurementNoise measurementNoise;
    Estimation.MocapExternalOdometryErrorState.PoseMeasurementNoise zeroMeasurementNoise;
    Estimation.MocapExternalOdometryErrorState.State initialized;
    Estimation.MocapExternalOdometryErrorState.State moving;
    Estimation.MocapExternalOdometryErrorState.State zeroDtPrediction;
    Estimation.MocapExternalOdometryErrorState.State predicted;
    Estimation.MocapExternalOdometryErrorState.State corrected;
    Estimation.MocapExternalOdometryErrorState.State signCorrected;
    Estimation.MocapExternalOdometryErrorState.State zeroState;
    Estimation.MocapExternalOdometryErrorState.State rejectedState;
    Estimation.MocapExternalOdometryErrorState.State skippedState;
    Estimation.MocapExternalOdometryErrorState.NominalState injected;
    Estimation.MocapExternalOdometryErrorState.Covariance transition;
    Estimation.MocapExternalOdometryErrorState.Covariance discreteNoise;
    Estimation.MocapExternalOdometryErrorState.MeasurementMatrix measurementMatrix;
    Real expectedAttitudeTransition[3, 3];
    Boolean accepted;
    Boolean signAccepted;
    Boolean rejected;
    Boolean skippedAccepted;
  algorithm
  initialVariances := Estimation.MocapExternalOdometryErrorState.InitialVariances(
    attitude=0.04,
    velocity=0.09,
    position=0.16,
    angularVelocity=0.25);
  zeroVariances := Estimation.MocapExternalOdometryErrorState.InitialVariances(
    attitude=0.0,
    velocity=0.0,
    position=0.0,
    angularVelocity=0.0);
  noProcessNoise := Estimation.MocapExternalOdometryErrorState.ProcessNoise(
    attitude=0.0,
    velocity=0.0,
    position=0.0,
    angularVelocity=0.0);
  processNoise := Estimation.MocapExternalOdometryErrorState.ProcessNoise(
    attitude=1.0e-5,
    velocity=2.0e-4,
    position=3.0e-6,
    angularVelocity=4.0e-5);
  measurementNoise := Estimation.MocapExternalOdometryErrorState.PoseMeasurementNoise(
    attitude=1.0e-4,
    position=4.0e-4);
  zeroMeasurementNoise := Estimation.MocapExternalOdometryErrorState.PoseMeasurementNoise(
    attitude=0.0,
    position=0.0);

  initialized := Estimation.MocapExternalOdometryErrorState.initialize(
    {1.0, -2.0, 3.0},
    {2.0, 0.0, 0.0, 0.0},
    initialVariances);
  moving.attitude := initialized.attitude;
  moving.velocity := {2.0, -1.0, 0.5};
  moving.position := initialized.position;
  moving.angularVelocity := {0.1, -0.2, 0.3};
  moving.covariance := initialized.covariance;
  zeroDtPrediction := Estimation.MocapExternalOdometryErrorState.predict(
    moving,
    0.0,
    processNoise);
  predicted := Estimation.MocapExternalOdometryErrorState.predict(
    moving,
    0.1,
    processNoise);
  transition := Estimation.MocapExternalOdometryErrorState.tangentTransition(
    0.1, moving.angularVelocity);
  expectedAttitudeTransition := transpose(LieGroups.SO3.Quat.to_DCM(
    LieGroups.SO3.Quat.exp_map(0.1 * moving.angularVelocity)));
  discreteNoise := Estimation.MocapExternalOdometryErrorState.discreteProcessCovariance(
    0.1, processNoise);
  measurementMatrix := Estimation.MocapExternalOdometryErrorState.poseMeasurementMatrix();

  exactMeasurement := Estimation.MocapExternalOdometryErrorState.PoseMeasurement(
    position=predicted.position,
    attitude=predicted.attitude);
  negativeQuaternionMeasurement := Estimation.MocapExternalOdometryErrorState.PoseMeasurement(
    position=predicted.position,
    attitude=-predicted.attitude);
  (corrected, accepted) := Estimation.MocapExternalOdometryErrorState.correct(
    predicted,
    exactMeasurement,
    measurementNoise);
  (signCorrected, signAccepted) := Estimation.MocapExternalOdometryErrorState.correct(
    predicted,
    negativeQuaternionMeasurement,
    measurementNoise);

  zeroState := Estimation.MocapExternalOdometryErrorState.initialize(
    zeros(3),
    {1.0, 0.0, 0.0, 0.0},
    zeroVariances);
  (rejectedState, rejected) := Estimation.MocapExternalOdometryErrorState.correct(
    zeroState,
    Estimation.MocapExternalOdometryErrorState.PoseMeasurement(
      position=zeros(3),
      attitude={1.0, 0.0, 0.0, 0.0}),
    zeroMeasurementNoise);
  (skippedState, skippedAccepted) := Estimation.MocapExternalOdometryErrorState.step(
    moving,
    0.1,
    false,
    exactMeasurement,
    processNoise,
    measurementNoise);
  injected := Estimation.MocapExternalOdometryErrorState.inject(
    Estimation.MocapExternalOdometryErrorState.NominalState(
      attitude=moving.attitude,
      velocity=moving.velocity,
      position=moving.position,
      angularVelocity=moving.angularVelocity),
    {0.0, 0.0, 0.1, 1.0, 2.0, 3.0, -1.0, -2.0, -3.0, 0.4, 0.5, 0.6});

  assert(Tests.Assertions.maxAbsVector(initialized.attitude - {1, 0, 0, 0}) < tolerance,
    "geometric error-state EKF initialization did not normalize attitude");
  assert(Tests.Assertions.maxAbsVector(initialized.velocity) < tolerance,
    "geometric error-state EKF initialization velocity was not zero");
  assert(Tests.Assertions.maxAbsVector(initialized.position - {1, -2, 3}) < tolerance,
    "geometric error-state EKF initialization position was incorrect");
  assert(abs(initialized.covariance[1, 1] - 0.04) < tolerance and
         abs(initialized.covariance[4, 4] - 0.09) < tolerance and
         abs(initialized.covariance[7, 7] - 0.16) < tolerance and
         abs(initialized.covariance[10, 10] - 0.25) < tolerance,
    "geometric error-state EKF initialization covariance ordering was incorrect");
  assert(Tests.Assertions.maxAbsVector(zeroDtPrediction.position - moving.position) < tolerance and
         Tests.Assertions.maxAbsMatrix(zeroDtPrediction.covariance - moving.covariance) < tolerance,
    "geometric error-state EKF zero-duration prediction was not an identity");
  assert(Tests.Assertions.maxAbsVector(
      predicted.position - (moving.position + 0.1 * moving.velocity)) < tolerance,
    "geometric error-state EKF constant-velocity position prediction failed");
  assert(Tests.Assertions.maxAbsMatrix(
           transition[1:3, 1:3] - expectedAttitudeTransition) < tolerance and
         abs(transition[7, 4] - 0.1) < tolerance and
         abs(discreteNoise[1, 1] - 0.1 * processNoise.attitude) < tolerance and
         abs(measurementMatrix[1, 1] - 1.0) < tolerance and
         abs(measurementMatrix[4, 7] - 1.0) < tolerance,
    "Separated estimator transition/noise/measurement maps were inconsistent");
  assert(abs(predicted.attitude * predicted.attitude - 1.0) < tolerance,
    "geometric error-state EKF prediction did not preserve quaternion norm");
  assert(Tests.Assertions.maxAbsMatrix(
      predicted.covariance - transpose(predicted.covariance)) < tolerance,
    "geometric error-state EKF prediction covariance was not symmetric");
  assert(accepted and signAccepted,
    "geometric error-state EKF rejected a positive-definite innovation covariance");
  assert(Tests.Assertions.maxAbsVector(corrected.position - predicted.position) < tolerance and
         Tests.Assertions.maxAbsVector(signCorrected.position - predicted.position) < tolerance,
    "A zero-residual geometric error-state EKF correction changed the nominal position");
  assert(abs(abs(corrected.attitude * predicted.attitude) - 1.0) < tolerance and
         abs(abs(signCorrected.attitude * predicted.attitude) - 1.0) < tolerance,
    "A zero-residual or sign-equivalent quaternion correction changed attitude");
  assert(corrected.covariance[1, 1] < predicted.covariance[1, 1] and
         corrected.covariance[7, 7] < predicted.covariance[7, 7],
    "geometric error-state EKF correction did not reduce observed covariance");
  assert(Tests.Assertions.maxAbsMatrix(
      corrected.covariance - transpose(corrected.covariance)) < tolerance,
    "geometric error-state EKF corrected covariance was not symmetric");
  assert(not rejected and Tests.Assertions.maxAbsMatrix(
         rejectedState.covariance - zeroState.covariance) < tolerance,
    "geometric error-state EKF singular innovation handling failed");
  assert(not skippedAccepted and Tests.Assertions.maxAbsMatrix(
         skippedState.covariance - predicted.covariance) < tolerance,
    "geometric error-state EKF skipped correction did not return prediction");
  assert(Tests.Assertions.maxAbsVector(injected.velocity - (moving.velocity + {1, 2, 3})) < tolerance and
         Tests.Assertions.maxAbsVector(injected.position - (moving.position - {1, 2, 3})) < tolerance and
         Tests.Assertions.maxAbsVector(injected.angularVelocity - (moving.angularVelocity + {0.4, 0.5, 0.6})) < tolerance,
    "geometric error-state EKF tangent injection ordering failed");
  for i in 1:12 loop
    assert(corrected.covariance[i, i] >= -tolerance,
      "geometric error-state EKF corrected covariance had a negative diagonal");
  end for;
  passed := true;
  end run;

  parameter Boolean passed = run();
equation
  assert(passed, "Estimator assertion function did not complete");
end EstimationTests;