Estimation.StrapdownINS.ESKF.correctLinear

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

function correctLinear
  "Vectorized Joseph-form correction in the local right-error tangent space"
  input State predicted;
  input Real residual[:];
  input Real H[size(residual, 1), TangentLength];
  input Real measurementCovariance[size(residual, 1), size(residual, 1)];
  input Real innovationGate = 0.0
    "Reject when NIS exceeds innovationGate * size(residual, 1);
     non-positive disables the gate";
  input Real attitudeGainAxis[3] = zeros(3)
    "Zero, the default, leaves the optimal gain alone. A UNIT body axis
     instead projects the three rotation rows of the gain onto that axis,
     so this measurement may rotate the state about it and about nothing
     else, while the rotation directions it is excluded from still carry
     their full sensitivity into the innovation covariance and into the
     Joseph update. That is a Schmidt, or consider, treatment: the excluded
     directions keep their uncertainty, the included one inherits the
     correlation the measurement really has with them, and the posterior
     stays consistent because the Joseph form below is valid for an
     ARBITRARY gain. It exists for a measurement whose Jacobian is honestly
     non-zero on a state the sensor must not be trusted to correct.";
  input Real measurementStateCrossCovariance[
    TangentLength, size(residual, 1)] =
    zeros(TangentLength, size(residual, 1))
    "Cov(error before correction, observation noise); zero for independent noise";
  input Boolean headingOnly = false
    "With a unit attitudeGainAxis, update only heading and gyro bias along that axis";
  input Boolean useSemiDirectBias = false;
  input Real measurementBarometerCrossCovariance[size(residual, 1)] =
    zeros(size(residual, 1));
  output State corrected;
  output Boolean accepted;
  output Integer rejectionReason
    "Estimation.StrapdownINS.Correction* code; Accepted when the
     correction was applied, and a named rejection cause otherwise";
  output Real normalizedInnovationSquared
    "Innovation residual squared after whitening by its predicted covariance";
protected
  Integer measurementLength = size(residual, 1);
  Boolean residualFinite;
  Boolean measurementCovarianceUsable;
  Boolean correlatedMeasurement;
  Boolean gatePassed;
  Real crossCovariance[TangentLength, size(residual, 1)];
  Real innovationCovariance[size(residual, 1), size(residual, 1)];
  Real augmentedRhs[size(residual, 1), TangentLength + 2];
  Real augmentedSolution[size(residual, 1), TangentLength + 2];
  Real gain[TangentLength, size(residual, 1)];
  Boolean factorized;
  TangentVector correction;
  Real josephFactor[TangentLength, TangentLength];
  Real posteriorCovariance[TangentLength, TangentLength];
  Real resetRotationJacobian[9, 9];
  Real resetJacobian[TangentLength, TangentLength];
  NominalState nominal;
  NominalState correctedNominal;
  Covariance correctedCovariance;
  Covariance correctedRoot;
  Real correctedBarometerCrossCovariance[TangentLength];
  Real correctedBarometerBias_m;
  Real correctedBarometerVariance_m2;
  Real barometerInnovationCrossCovariance[size(residual, 1)];
  Real barometerGain[size(residual, 1)];
  Real barometerNavigationErrorMap[TangentLength];
  Real navigationBarometerCrossCovariance[TangentLength];
  Real noiseBarometerCrossCovariance[size(residual, 1)];
  Real barometerErrorCovariance;
  Real noiseStateRoot[TangentLength, size(residual, 1)];
  Real measurementColumns[size(residual, 1), TangentLength];
  Real measurementRoot[size(residual, 1), size(residual, 1)];
  Real innovationRoot[size(residual, 1), size(residual, 1)];
  Covariance priorRoot;
  Real whitened[size(residual, 1), TangentLength + 2];
  Boolean crossFactorized;
  Boolean noiseFactorized;
algorithm
  residualFinite := true;
  for row in 1:measurementLength loop
    residualFinite := residualFinite
      and abs(residual[row]) < FiniteMagnitudeLimit;
  end for;

  measurementCovarianceUsable := true;
  for row in 1:measurementLength loop
    measurementCovarianceUsable := measurementCovarianceUsable
      and measurementCovariance[row, row] > 0.0
      and measurementCovariance[row, row] < FiniteMagnitudeLimit
      and abs(measurementBarometerCrossCovariance[row]) < FiniteMagnitudeLimit;
    for column in 1:measurementLength loop
      measurementCovarianceUsable := measurementCovarianceUsable
        and abs(measurementCovariance[row, column]) < FiniteMagnitudeLimit;
    end for;
  end for;

  correlatedMeasurement := false;
  for row in 1:TangentLength loop
    measurementCovarianceUsable := measurementCovarianceUsable
      and abs(predicted.barometerBiasCrossCovariance[row]) < FiniteMagnitudeLimit;
    for column in 1:measurementLength loop
      measurementCovarianceUsable := measurementCovarianceUsable
        and abs(measurementStateCrossCovariance[row, column]) < FiniteMagnitudeLimit;
      correlatedMeasurement := correlatedMeasurement
        or measurementStateCrossCovariance[row, column] <> 0.0;
    end for;
  end for;

  if predicted.useJointBarometerBias then
    measurementCovarianceUsable := measurementCovarianceUsable
      and abs(predicted.barometerBias_m) < FiniteMagnitudeLimit
      and predicted.barometerBiasVariance_m2 >= 0.0
      and predicted.barometerBiasVariance_m2 < FiniteMagnitudeLimit;
  end if;
  correctedRoot := predicted.covarianceRoot;
  correctedBarometerCrossCovariance := predicted.barometerBiasCrossCovariance;
  correctedBarometerBias_m := predicted.barometerBias_m;
  correctedBarometerVariance_m2 := predicted.barometerBiasVariance_m2;
  barometerInnovationCrossCovariance := H * predicted.barometerBiasCrossCovariance
    + measurementBarometerCrossCovariance;
  measurementColumns := zeros(measurementLength, TangentLength);
  measurementRoot := zeros(measurementLength, measurementLength);
  if predicted.useSquareRootCovariance then
    (noiseStateRoot, crossFactorized) := LinearAlgebra.solveLower(
      predicted.covarianceRoot, measurementStateCrossCovariance);
    (measurementRoot, noiseFactorized) := LinearAlgebra.factorPSD(
      measurementCovariance - transpose(noiseStateRoot) * noiseStateRoot);
    measurementColumns := H * predicted.covarianceRoot
      + transpose(noiseStateRoot);
    crossCovariance := predicted.covarianceRoot * transpose(measurementColumns);
    innovationRoot := LinearAlgebra.covarianceRoot(
      cat(2, measurementColumns, measurementRoot));
    augmentedRhs := zeros(measurementLength, TangentLength + 2);
    augmentedRhs[:, 1:TangentLength] := transpose(crossCovariance);
    augmentedRhs[:, TangentLength + 1] := residual;
    augmentedRhs[:, TangentLength + 2] := barometerInnovationCrossCovariance;
    (augmentedSolution, whitened, factorized) :=
      LinearAlgebra.solveCovarianceRoot(innovationRoot, augmentedRhs);
    factorized := factorized and crossFactorized and noiseFactorized;
    normalizedInnovationSquared := whitened[:, TangentLength + 1]
      * whitened[:, TangentLength + 1];
  else
    crossFactorized := true;
    noiseStateRoot := zeros(TangentLength, measurementLength);
    if correlatedMeasurement then
      (priorRoot, crossFactorized) := LinearAlgebra.factorPSD(predicted.covariance);
      (noiseStateRoot, noiseFactorized) := LinearAlgebra.solveLower(
        priorRoot, measurementStateCrossCovariance);
      crossFactorized := crossFactorized and noiseFactorized;
    end if;
    (measurementRoot, noiseFactorized) := LinearAlgebra.factorPSD(
      measurementCovariance
        - transpose(noiseStateRoot) * noiseStateRoot);
    measurementCovarianceUsable := measurementCovarianceUsable
      and crossFactorized and noiseFactorized;
    crossCovariance := predicted.covariance * transpose(H)
      + measurementStateCrossCovariance;
    innovationCovariance := LinearAlgebra.symmetrize(
      H * crossCovariance
        + transpose(measurementStateCrossCovariance) * transpose(H)
        + measurementCovariance);
    augmentedRhs := zeros(measurementLength, TangentLength + 2);
    augmentedRhs[:, 1:TangentLength] := transpose(crossCovariance);
    augmentedRhs[:, TangentLength + 1] := residual;
    augmentedRhs[:, TangentLength + 2] := barometerInnovationCrossCovariance;
    (augmentedSolution, factorized) := LinearAlgebra.solveSPD(
      innovationCovariance, augmentedRhs);
    normalizedInnovationSquared :=
      residual * augmentedSolution[:, TangentLength + 1];
  end if;
  gatePassed := normalizedInnovationSquared >= 0.0
    and normalizedInnovationSquared < FiniteMagnitudeLimit
    and (innovationGate <= 0.0
      or normalizedInnovationSquared <= innovationGate * measurementLength);
  accepted := factorized and residualFinite and measurementCovarianceUsable
    and gatePassed;
  // Every rejection carries a named cause. Ordered by depth: an unusable
  // measurement covariance or a non-finite residual makes the
  // factorization and the gate result meaningless, so those are reported
  // first and the deeper verdicts are not attributed.
  rejectionReason := if accepted then CorrectionAccepted
    elseif not residualFinite then CorrectionRejectedNotFinite
    elseif not measurementCovarianceUsable then
      CorrectionRejectedCovarianceUnusable
    elseif not factorized then CorrectionRejectedFactorization
    else CorrectionRejectedGate;
  if accepted then
    (gain, correction) := constrainGain(
      transpose(augmentedSolution[:, 1:TangentLength]), residual,
      attitudeGainAxis, headingOnly);
    correctedBarometerCrossCovariance := predicted.barometerBiasCrossCovariance
      - gain * barometerInnovationCrossCovariance;
    if predicted.useJointBarometerBias then
      barometerGain := augmentedSolution[:, TangentLength + 2];
      barometerNavigationErrorMap := -transpose(H) * barometerGain;
      navigationBarometerCrossCovariance :=
        predicted.covariance * barometerNavigationErrorMap
        + predicted.barometerBiasCrossCovariance
        - measurementStateCrossCovariance * barometerGain;
      noiseBarometerCrossCovariance :=
        transpose(measurementStateCrossCovariance) * barometerNavigationErrorMap
        + measurementBarometerCrossCovariance - measurementCovariance * barometerGain;
      barometerErrorCovariance := predicted.barometerBiasCrossCovariance
        * barometerNavigationErrorMap + predicted.barometerBiasVariance_m2
        - measurementBarometerCrossCovariance * barometerGain;
      correctedBarometerCrossCovariance :=
        (identity(TangentLength) - gain * H)
          * navigationBarometerCrossCovariance - gain * noiseBarometerCrossCovariance;
      correctedBarometerVariance_m2 :=
        barometerNavigationErrorMap * navigationBarometerCrossCovariance
        + barometerErrorCovariance - barometerGain * noiseBarometerCrossCovariance;
      correctedBarometerBias_m := predicted.barometerBias_m + barometerGain * residual;
    end if;

    nominal := NominalState(
      positionWorldEnu_m=predicted.positionWorldEnu_m,
      velocityWorldEnu_m_s=predicted.velocityWorldEnu_m_s,
      quaternionWorldBody=predicted.quaternionWorldBody,
      gyroscopeBiasBodyFlu_rad_s=predicted.gyroscopeBiasBodyFlu_rad_s,
      accelerometerBiasBodyFlu_m_s2=predicted.accelerometerBiasBodyFlu_m_s2);
    if useSemiDirectBias then
      correctedNominal := SemiDirectBias.inject(nominal, correction);
    else
      correctedNominal := inject(nominal, correction);
    end if;
    if predicted.useSquareRootCovariance then
      correctedRoot := LinearAlgebra.covarianceRoot(cat(2,
        predicted.covarianceRoot - gain * measurementColumns,
        gain * measurementRoot));
      resetJacobian := identity(TangentLength);
      if useSemiDirectBias then
        resetJacobian := SemiDirectBias.resetJacobian(correction);
      else
        resetRotationJacobian :=
          LieGroups.SE23.Quat.right_jacobian(correction[1:9]);
        resetJacobian := cat(1,
          cat(2, resetRotationJacobian, zeros(9, 6)),
          cat(2, zeros(6, 9), identity(6)));
      end if;
      correctedRoot := LinearAlgebra.covarianceRoot(resetJacobian * correctedRoot);
      correctedBarometerCrossCovariance :=
        resetJacobian * correctedBarometerCrossCovariance;
      correctedCovariance := LinearAlgebra.symmetrize(
        correctedRoot * transpose(correctedRoot));
    else
      josephFactor := identity(TangentLength) - gain * H;
      posteriorCovariance := LinearAlgebra.symmetrize(
        LinearAlgebra.josephUpdate(
          josephFactor,
          predicted.covariance,
          gain,
          measurementCovariance));
      if correlatedMeasurement then
        // Cov((I-KH)e - K v) includes both cross terms when Cov(e,v) != 0.
        // This generalized Joseph form remains valid for the effective gain
        // after the attitude trust limit and consider projection above.
        posteriorCovariance := LinearAlgebra.symmetrize(posteriorCovariance
          - josephFactor * measurementStateCrossCovariance * transpose(gain)
          - gain * transpose(measurementStateCrossCovariance)
            * transpose(josephFactor));
      end if;
      if useSemiDirectBias then
        resetJacobian := SemiDirectBias.resetJacobian(correction);
        correctedCovariance := LinearAlgebra.symmetrize(
          LinearAlgebra.transformCovariance(resetJacobian, posteriorCovariance));
        correctedBarometerCrossCovariance :=
          resetJacobian * correctedBarometerCrossCovariance;
      else
        // The reset Jacobian is the block diagonal diag(J, identity(6)) with
        // literal zeros off the diagonal, so the conjugation is done blockwise in
        // conjugateReset rather than by forming the 15x15 matrix and multiplying
        // it through: 54,000 -> 2,431 multiplies per call, same products, same
        // order. See conjugateReset for the exactness argument, the two
        // exceptional-value changes, and why it has to be a separate function.
        resetRotationJacobian := LieGroups.SE23.Quat.right_jacobian(correction[1:9]);
        correctedCovariance := LinearAlgebra.symmetrize(
          conjugateReset(
            resetRotationJacobian, posteriorCovariance));
        correctedBarometerCrossCovariance := cat(1,
          resetRotationJacobian * correctedBarometerCrossCovariance[1:9],
          correctedBarometerCrossCovariance[10:TangentLength]);
      end if;
    end if;
  else
    // A rejected correction (factorization failure or gate) never
    // modifies the state.
    correctedNominal := NominalState(
      positionWorldEnu_m=predicted.positionWorldEnu_m,
      velocityWorldEnu_m_s=predicted.velocityWorldEnu_m_s,
      quaternionWorldBody=predicted.quaternionWorldBody,
      gyroscopeBiasBodyFlu_rad_s=predicted.gyroscopeBiasBodyFlu_rad_s,
      accelerometerBiasBodyFlu_m_s2=
        predicted.accelerometerBiasBodyFlu_m_s2);
    correctedCovariance := predicted.covariance;
  end if;
  corrected := State(
    positionWorldEnu_m=correctedNominal.positionWorldEnu_m,
    velocityWorldEnu_m_s=correctedNominal.velocityWorldEnu_m_s,
    quaternionWorldBody=correctedNominal.quaternionWorldBody,
    gyroscopeBiasBodyFlu_rad_s=
      correctedNominal.gyroscopeBiasBodyFlu_rad_s,
    accelerometerBiasBodyFlu_m_s2=
      correctedNominal.accelerometerBiasBodyFlu_m_s2,
    covariance=correctedCovariance,
    useSquareRootCovariance=predicted.useSquareRootCovariance,
    covarianceRoot=correctedRoot,
    barometerBiasCrossCovariance=correctedBarometerCrossCovariance,
    barometerBias_m=correctedBarometerBias_m,
    barometerBiasVariance_m2=correctedBarometerVariance_m2,
    useJointBarometerBias=predicted.useJointBarometerBias);
end correctLinear;