Tests.StrapdownPreintegrationJacobianTests.run

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

function run
    output Boolean passed;
  protected
    constant Real epsilon = 1.0e-5;
    constant Real anchor[6] = {0.02, -0.01, 0.03, 0.1, -0.2, 0.05};
    Real nominal[10];
    Real plus[10];
    Real minus[10];
    Real J[9, 6];
    Real scratch[9, 6];
    Real numerical[9, 6];
    Real perturbation[6];
    Real inverseQuaternion[4];
  algorithm
    // Both holds: zero rotation, either side of the coefficient-series branch,
    // and a large three-axis varying input. A nonidentity initial pose and
    // three intervals exercise transport of all five accumulated bias blocks.
    for holdIndex in 1:2 loop
      for caseIndex in 1:4 loop
        (nominal, J) := compose(caseIndex, holdIndex == 2, anchor);
        inverseQuaternion := LieGroups.SO3.Quat.inverse(nominal[7:10]);
        for column in 1:6 loop
          perturbation := {if row == column then epsilon else 0.0 for row in 1:6};
          (plus, scratch) := compose(caseIndex, holdIndex == 2,
            anchor + perturbation);
          (minus, scratch) := compose(caseIndex, holdIndex == 2,
            anchor - perturbation);
          numerical[1:6, column] := (plus[1:6] - minus[1:6]) / (2.0 * epsilon);
          numerical[7:9, column] := (
            LieGroups.SO3.Quat.log_map(LieGroups.SO3.Quat.product(
              inverseQuaternion, plus[7:10]))
            - LieGroups.SO3.Quat.log_map(LieGroups.SO3.Quat.product(
              inverseQuaternion, minus[7:10]))) / (2.0 * epsilon);
        end for;
        assert(Tests.Assertions.maxAbsMatrix(J - numerical) < 2.0e-8,
          "Composed IMU bias Jacobian disagrees with central differences");
      end for;
    end for;
    passed := true;
  end run;