SLAM.Localization.RGBDLocalizationCatalog.ValidEstimator

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

function ValidEstimator
    input Estimator value;
    output Boolean valid;
  protected
    Real joint[currentDimension+referenceDimension,currentDimension+referenceDimension];
  algorithm
    valid := RGBDMapAnchors.ProperRotation(value.rotation)
      and SLAMCovariancePSDCheck(value.covariance,1e-12) == 1.0
      and (SLAMExactRealEqual(value.referenceAvailable,0.0) or SLAMExactRealEqual(value.referenceAvailable,1.0))
      and (SLAMExactRealEqual(value.referenceUsed,0.0) or SLAMExactRealEqual(value.referenceUsed,1.0))
      and value.lastUsedEpoch >= -1.0 and value.lastUsedEpoch <= identifierLimit
      and SLAMExactRealEqual(value.lastUsedEpoch,floor(value.lastUsedEpoch));
    for axis in 1:dimension loop
      valid := valid and abs(value.position[axis]) <= 1e6 and abs(value.velocity[axis]) <= 1e6
        and abs(value.accelBias[axis]) <= 2.0 and abs(value.gyroBias[axis]) <= 0.3;
    end for;
    // Unavailable reference buffers stay opaque, matching the propagation owner.
    if SLAMExactRealEqual(value.referenceAvailable,0.0) then
      valid := valid and SLAMExactRealEqual(value.referenceUsed,0.0);
    elseif SLAMExactRealEqual(value.referenceAvailable,1.0) then
      valid := valid and RGBDMapAnchors.ProperRotation(value.referenceRotation)
        and value.referenceEpoch >= 0.0 and value.referenceEpoch <= identifierLimit
        and SLAMExactRealEqual(value.referenceEpoch,floor(value.referenceEpoch))
        and ((SLAMExactRealEqual(value.referenceUsed,0.0) and value.referenceEpoch > value.lastUsedEpoch)
          or (SLAMExactRealEqual(value.referenceUsed,1.0) and value.referenceEpoch <= value.lastUsedEpoch));
      for axis in 1:dimension loop
        valid := valid and abs(value.referencePosition[axis]) <= 1e6;
      end for;
      joint := cat(1,cat(2,value.covariance,value.crossCovariance),
        cat(2,transpose(value.crossCovariance),value.referenceCovariance));
      valid := valid and SLAMCovariancePSDCheck(joint,1e-12) == 1.0;
    end if;
  end ValidEstimator;