function

Tests.RuleChecks.retrodictTangentError

Delayed-tangent error retrodict produces for one perturbation of the current state

Public declarations

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

Role	Name	Type	Dimensions / attributes / default	Description
input	`current`	`Estimation.StrapdownINS.ESKF.State`
input	`angularVelocityMeasuredBodyFlu_rad_s`	`Real`	`[3]`
input	`specificForceMeasuredBodyFlu_m_s2`	`Real`	`[3]`
input	`gravityWorldEnu_m_s2`	`Real`	`[3]`
input	`age_s`	`Real`	`(unit = "s")`
input	`perturbation`	`Real`	`[15]`	Current-tangent perturbation: pose on the right, biases additive
input	`baseInverse`	`Real`	`[10]`	Inverse of the unperturbed delayed extended pose
input	`baseBiases`	`Real`	`[6]`	Unperturbed delayed gyroscope and accelerometer biases
output	`tangentError`	`Real`	`[15]`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function retrodictTangentError
  "Delayed-tangent error retrodict produces for one perturbation of the current state"
  input Estimation.StrapdownINS.ESKF.State current;
  input Real angularVelocityMeasuredBodyFlu_rad_s[3];
  input Real specificForceMeasuredBodyFlu_m_s2[3];
  input Real gravityWorldEnu_m_s2[3];
  input Real age_s(unit = "s");
  input Real perturbation[15]
    "Current-tangent perturbation: pose on the right, biases additive";
  input Real baseInverse[10] "Inverse of the unperturbed delayed extended pose";
  input Real baseBiases[6] "Unperturbed delayed gyroscope and accelerometer biases";
  output Real tangentError[15];
protected
  Real perturbedPose[10];
  Real perturbedVector[16];
algorithm
  perturbedPose := LieGroups.SE23.Quat.product(
    cat(1, current.positionWorldEnu_m, current.velocityWorldEnu_m_s,
      current.quaternionWorldBody),
    LieGroups.SE23.Quat.exp_map(perturbation[1:9]));
  perturbedVector := Estimation.StrapdownINS.ESKF.retrodict(
    Estimation.StrapdownINS.ESKF.State(
      barometerBiasCrossCovariance=zeros(15),
      barometerBias_m=0.0, barometerBiasVariance_m2=0.0, useJointBarometerBias=false,
      positionWorldEnu_m = perturbedPose[1:3],
      velocityWorldEnu_m_s = perturbedPose[4:6],
      quaternionWorldBody = perturbedPose[7:10],
      gyroscopeBiasBodyFlu_rad_s = current.gyroscopeBiasBodyFlu_rad_s
        + perturbation[10:12],
      accelerometerBiasBodyFlu_m_s2 = current.accelerometerBiasBodyFlu_m_s2
        + perturbation[13:15],
      covariance = zeros(15, 15),
        useSquareRootCovariance=false, covarianceRoot=zeros(15, 15)),
    angularVelocityMeasuredBodyFlu_rad_s,
    specificForceMeasuredBodyFlu_m_s2, gravityWorldEnu_m_s2, age_s);
  tangentError := cat(1,
    LieGroups.SE23.Quat.log_map(LieGroups.SE23.Quat.product(
      baseInverse, perturbedVector[1:10])),
    perturbedVector[11:16] - baseBiases);
end retrodictTangentError;