function

Estimation.StrapdownINS.ESKF.seedPositionVariances

Bound an aiding position seed's uncertainty in the local tangent frame

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`covarianceWorld_m2`	`Real`	`[3, 3]`
input	`quaternionWorldBody`	`Real`	`[4]`
input	`configuredVariances_m2`	`Real`	`[3]`
output	`variances_m2`	`Real`	`[3]`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function seedPositionVariances
  "Bound an aiding position seed's uncertainty in the local tangent frame"
  input Real covarianceWorld_m2[3, 3];
  input Real quaternionWorldBody[4];
  input Real configuredVariances_m2[3];
  output Real variances_m2[3];
protected
  Boolean covarianceFinite;
  Boolean covariancePositive;
  Real probe[3, 1];
  Real rotationWorldBody[3, 3];
  Real covarianceBody_m2[3, 3];
algorithm
  variances_m2 := configuredVariances_m2;
  covarianceFinite := true;
  for row in 1:3 loop
    for column in 1:3 loop
      covarianceFinite := covarianceFinite
        and abs(covarianceWorld_m2[row, column]) < FiniteMagnitudeLimit;
    end for;
  end for;
  if covarianceFinite then
    (probe, covariancePositive) := LinearAlgebra.solveSPD(
      LinearAlgebra.symmetrize(covarianceWorld_m2), zeros(3, 1));
    if covariancePositive then
      rotationWorldBody := LieGroups.SO3.Quat.to_DCM(
        LieGroups.SO3.Quat.normalize(quaternionWorldBody));
      covarianceBody_m2 := transpose(rotationWorldBody)
        * LinearAlgebra.symmetrize(covarianceWorld_m2) * rotationWorldBody;
      for row in 1:3 loop
        // The initializer stores diagonal tangent variances. Absolute row
        // sums give a diagonal majorant of the full rotated covariance,
        // retaining configured floors and bounding off-diagonal terms.
        variances_m2[row] := max(configuredVariances_m2[row],
          sum(abs(covarianceBody_m2[row, :])));
      end for;
    end if;
  end if;
end seedPositionVariances;