function

Tests.MagneticVectorTests.linearizationError

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`angle`	`Real`
input	`field`	`Real`	`[3]`
output	`error`	`Real`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function linearizationError
    input Real angle;
    input Real field[3];
    output Real error;
  protected
    Real rotation[3];
    Real measured[3];
    Real residual[3];
    Real H[3, 3];
    Real R[3, 3];
    Boolean usable;
  algorithm
    rotation := angle * {0.3, -0.4, 0.5} / sqrt(0.5);
    measured := transpose(LieGroups.SO3.Quat.to_DCM(
      LieGroups.SO3.Quat.exp_map(rotation))) * field;
    (residual, H, R, usable) :=
      Estimation.StrapdownINS.magnetometerVectorObservation(
        {1, 0, 0, 0}, measured, 9.0e-14 * identity(3), field);
    assert(usable, "Finite magnetic vector observation was rejected");
    error := sqrt((residual - H * rotation) * (residual - H * rotation));
  end linearizationError;