SLAM.Inertial.SchmidtGraphPoseCorrection.Correct

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

function Correct
    input State previous;
    input GraphGaugeUncertainty.Estimate graph;
    input GraphGaugeUncertainty.Binding expectedBinding;
    input Attempt previousAttempt;
    input Policy policy;
    input Boolean requested;
    output Result result;
  protected
    Boolean valid; Boolean logValid; Boolean solveValid; Boolean sameCapture;
    Integer count; Integer offset; Integer stateOffset;
    Real prior[21,21]; Real weightedPrior[21,21]; Real Q[12,12]; Real noiseMap[12,12];
    Real H[12,21]; Real innovation[12]; Real graphRotation[3,3]; Real priorRotation[3,3];
    Real logVector[3]; Real leftInverse[3,3]; Real rightInverse[3,3];
    Real cross[21,12]; Real S[12,12]; Real rhs[12,22]; Real solved[12,22];
    Real K[21,12]; Real delta[21]; Real A[21,21]; Real beforeReset[21,21];
    Real reset[21,21]; Real proposedCovariance[21,21]; State candidate;
  algorithm
    result.next := previous; result.accepted := false; result.reason := 1;
    result.attempted := false; result.nextAttempt := previousAttempt;
    result.innovation := zeros(12); result.measurementJacobian := zeros(12,21);
    result.noise := zeros(12,12); result.gain := zeros(21,12); result.correction := zeros(21);
    result.nis := 0; result.covarianceBeforeReset := zeros(21,21); result.resetJacobian := identity(21);
    if requested then
      sameCapture := GraphGaugeUncertainty.SameCapture(graph.binding);
      result.reason := 2;
      valid := GraphGaugeUncertainty.ValidBinding(graph.binding)
        and GraphGaugeUncertainty.SameBinding(graph.binding,expectedBinding)
        and previous.generation == graph.binding.generation
        and previous.sourceRevision == graph.binding.sourceRevision
        and previous.currentId == graph.binding.currentId and previous.currentEpoch == graph.binding.currentEpoch
        and previous.currentCaptureSequence == graph.binding.currentCaptureSequence
        and previous.predictionTime == graph.binding.currentTime
        and (previous.referenceAvailable or not previous.referenceUsed)
        and previousAttempt.generation == previous.generation and previousAttempt.graphRevision >= 0
        and (if previousAttempt.graphRevision == 0 then previousAttempt.factorProvenance == 0
          else previousAttempt.factorProvenance > 0)
        and graph.binding.graphRevision > previousAttempt.graphRevision
        and graph.binding.factorProvenance <> previousAttempt.factorProvenance;
      if previous.referenceAvailable then
        valid := valid and previous.referenceId == graph.binding.referenceId
          and previous.referenceEpoch == graph.binding.referenceEpoch
          and previous.referenceCaptureSequence == graph.binding.referenceCaptureSequence
          and previous.referenceTime == graph.binding.referenceTime
          and previous.lastUsedEpoch >= -1 and previous.lastUsedEpoch <= previous.currentEpoch
          and (if previous.referenceUsed then previous.referenceEpoch <= previous.lastUsedEpoch
            else previous.referenceEpoch > previous.lastUsedEpoch);
      end if;
      if sameCapture then valid := valid and previous.referenceAvailable; end if;
      if valid then
        // Attempt metadata survives numerical refusal. The enclosing owner must
        // persist it separately from the rolled-back numerical tuple.
        result.attempted := true;
        result.nextAttempt.generation := previous.generation;
        result.nextAttempt.graphRevision := graph.binding.graphRevision;
        result.nextAttempt.factorProvenance := graph.binding.factorProvenance;
        result.reason := 3;
        valid := policy.weight > 0 and policy.weight < 1
          and policy.maximumPositionInnovation > 0 and policy.maximumPositionInnovation <= 1e6
          and policy.maximumAngularInnovation > 0 and policy.maximumAngularInnovation < 3.140592653589793
          and policy.maximumPositionCorrection > 0 and policy.maximumPositionCorrection <= 1e6
          and policy.maximumAngularCorrection > 0 and policy.maximumAngularCorrection < 3.140592653589793
          and policy.maximumNis > 0 and policy.maximumNis <= 1e12;
      end if;
      if valid then
        result.reason := 4;
        valid := RGBDUncertaintyProper(previous.rotation) and RGBDUncertaintyProper(graph.rotations[1,:,:]);
        for i in 1:3 loop
          valid := valid and abs(previous.position[i]) <= 1e6 and abs(previous.velocity[i]) <= 1e6
            and abs(previous.accelBias[i]) <= 2 and abs(previous.gyroBias[i]) <= 0.3
            and abs(graph.positions[1,i]) <= 1e6;
        end for;
        if previous.referenceAvailable then
          valid := valid and RGBDUncertaintyProper(previous.referenceRotation)
            and RGBDUncertaintyProper(graph.rotations[2,:,:]);
          for i in 1:3 loop
            valid := valid and abs(previous.referencePosition[i]) <= 1e6 and abs(graph.positions[2,i]) <= 1e6;
          end for;
        end if;
      end if;
      if valid then
        result.reason := 5; prior := zeros(21,21);
        prior[1:15,1:15] := previous.covariance;
        if previous.referenceAvailable then
          prior[1:15,16:21] := previous.crossCovariance;
          prior[16:21,1:15] := transpose(previous.crossCovariance);
          prior[16:21,16:21] := previous.referenceCovariance;
          valid := SLAMCovariancePSDCheck(prior,1e-12) == 1
            and SLAMCovariancePSDCheck(graph.covariance,1e-12) == 1;
        else
          valid := SLAMCovariancePSDCheck(previous.covariance,1e-12) == 1
            and SLAMCovariancePSDCheck(graph.covariance[1:6,1:6],1e-12) == 1;
        end if;
      end if;
      if valid and sameCapture then
        result.reason := 5;
        valid := ExactClone(previous) and GraphGaugeUncertainty.CloneEstimate(graph);
      end if;
      if valid then
        result.reason := 6; H := zeros(12,21); noiseMap := zeros(12,12); innovation := zeros(12);
        count := if previous.referenceAvailable and not sameCapture then 2 else 1;
        for node in 1:count loop
          offset := 6*(node-1); stateOffset := if node == 1 then 0 else 15;
          priorRotation := if node == 1 then previous.rotation else previous.referenceRotation;
          graphRotation := graph.rotations[node,:,:];
          (logVector,logValid) := Log(transpose(priorRotation)*graphRotation);
          valid := valid and logValid and sum(logVector.^2) <= policy.maximumAngularInnovation^2;
          innovation[offset+1:offset+3] := graph.positions[node,:]
            -(if node == 1 then previous.position else previous.referencePosition);
          innovation[offset+4:offset+6] := logVector;
          valid := valid and sum(innovation[offset+1:offset+3].^2) <= policy.maximumPositionInnovation^2;
          leftInverse := InverseJacobian(logVector,false); rightInverse := InverseJacobian(logVector,true);
          H[offset+1:offset+3,stateOffset+1:stateOffset+3] := identity(3);
          H[offset+4:offset+6,stateOffset+(if node == 1 then 7 else 4):stateOffset+(if node == 1 then 9 else 6)] := leftInverse;
          noiseMap[offset+1:offset+3,offset+1:offset+3] := identity(3);
          noiseMap[offset+4:offset+6,offset+4:offset+6] := rightInverse;
        end for;
      end if;
      if valid then
        Q := zeros(12,12);
        if previous.referenceAvailable and not sameCapture then
          Q := noiseMap*graph.covariance*transpose(noiseMap);
        else
          Q[1:6,1:6] := noiseMap[1:6,1:6]*graph.covariance[1:6,1:6]*transpose(noiseMap[1:6,1:6]);
          // Neutral solver padding for current-only or one same-capture pose,
          // neither an extra reference observation nor retained
          // covariance. Zero H/K rows make this identical to the six-row solve.
          Q[7:12,7:12] := identity(6);
        end if;
        weightedPrior := prior/policy.weight; cross := weightedPrior*transpose(H);
        S := H*cross+Q/(1-policy.weight);
        rhs[:,1:21] := transpose(cross); rhs[:,22] := innovation;
        (solved,solveValid) := Solve(S,rhs);
        result.reason := 7; valid := solveValid;
      end if;
      if valid then
        K := transpose(solved[:,1:21]);
        if policy.constrainVelocityAndBias then
          K[4:6,:] := zeros(3,12); K[10:15,:] := zeros(6,12);
        end if;
        delta := K*innovation;
        result.innovation := innovation; result.measurementJacobian := H; result.noise := Q;
        result.gain := K; result.correction := delta; result.nis := sum(innovation.*solved[:,22]);
        result.reason := 8;
        valid := result.nis >= 0 and result.nis <= policy.maximumNis;
        for i in 1:21 loop valid := valid and abs(delta[i]) <= 1e6; end for;
        valid := valid and sum(delta[1:3].^2) <= policy.maximumPositionCorrection^2
          and sum(delta[7:9].^2) <= policy.maximumAngularCorrection^2;
        if previous.referenceAvailable then
          valid := valid and sum(delta[16:18].^2) <= policy.maximumPositionCorrection^2
            and sum(delta[19:21].^2) <= policy.maximumAngularCorrection^2;
        end if;
      end if;
      if valid then
        candidate := previous;
        candidate.position := previous.position+delta[1:3]; candidate.velocity := previous.velocity+delta[4:6];
        candidate.rotation := previous.rotation*Exp(delta[7:9]);
        candidate.accelBias := previous.accelBias+delta[10:12]; candidate.gyroBias := previous.gyroBias+delta[13:15];
        if previous.referenceAvailable then
          candidate.referencePosition := previous.referencePosition+delta[16:18];
          candidate.referenceRotation := previous.referenceRotation*Exp(delta[19:21]);
        end if;
        result.reason := 9; valid := RGBDUncertaintyProper(candidate.rotation);
        for i in 1:3 loop
          valid := valid and abs(candidate.position[i]) <= 1e6 and abs(candidate.velocity[i]) <= 1e6
            and abs(candidate.accelBias[i]) <= 2 and abs(candidate.gyroBias[i]) <= 0.3;
        end for;
        if previous.referenceAvailable then
          valid := valid and RGBDUncertaintyProper(candidate.referenceRotation);
          for i in 1:3 loop valid := valid and abs(candidate.referencePosition[i]) <= 1e6; end for;
        end if;
      end if;
      if valid then
        A := identity(21)-K*H;
        beforeReset := A*weightedPrior*transpose(A)+K*(Q/(1-policy.weight))*transpose(K);
        reset := identity(21); reset[7:9,7:9] := RightJacobian(delta[7:9]);
        if previous.referenceAvailable then reset[19:21,19:21] := RightJacobian(delta[19:21]); end if;
        proposedCovariance := reset*beforeReset*transpose(reset);
        proposedCovariance := (proposedCovariance+transpose(proposedCovariance))/2;
        result.covarianceBeforeReset := beforeReset; result.resetJacobian := reset;
        result.reason := 10;
        valid := SLAMCovariancePSDCheck(proposedCovariance,1e-12) == 1;
        if valid and sameCapture then
          // Validate full21 result closure before the structural clone map
          // C=[I15; selected6]. Only NEW corrected Pcc/means are retained.
          for i in 1:3 loop
            valid := valid and abs(candidate.referencePosition[i]-candidate.position[i])
              <= 1e-11*(1+max(abs(candidate.referencePosition[i]),abs(candidate.position[i])));
            for j in 1:3 loop valid := valid and abs(candidate.referenceRotation[i,j]-candidate.rotation[i,j]) <= 1e-11; end for;
          end for;
          for i in 1:15 loop for j in 1:6 loop
            valid := valid and abs(proposedCovariance[i,j+15]-proposedCovariance[i,poseIndices[j]])
              <= 1e-11*(1+max(abs(proposedCovariance[i,j+15]),abs(proposedCovariance[i,poseIndices[j]])));
          end for; end for;
          for i in 1:6 loop for j in 1:6 loop
            valid := valid and abs(proposedCovariance[i+15,j+15]-proposedCovariance[poseIndices[i],poseIndices[j]])
              <= 1e-11*(1+max(abs(proposedCovariance[i+15,j+15]),abs(proposedCovariance[poseIndices[i],poseIndices[j]])));
          end for; end for;
          if valid then
            candidate.referencePosition := candidate.position; candidate.referenceRotation := candidate.rotation;
            for i in 1:15 loop for j in 1:6 loop
              proposedCovariance[i,j+15] := proposedCovariance[i,poseIndices[j]];
              proposedCovariance[j+15,i] := proposedCovariance[poseIndices[j],i];
            end for; end for;
            for i in 1:6 loop for j in 1:6 loop
              proposedCovariance[i+15,j+15] := proposedCovariance[poseIndices[i],poseIndices[j]];
            end for; end for;
            valid := SLAMCovariancePSDCheck(proposedCovariance,1e-12) == 1;
          end if;
        end if;
        if valid then
          candidate.covariance := proposedCovariance[1:15,1:15];
          if previous.referenceAvailable then
            candidate.crossCovariance := proposedCovariance[1:15,16:21];
            candidate.referenceCovariance := proposedCovariance[16:21,16:21];
          end if;
          result.next := candidate; result.accepted := true; result.reason := 0;
        end if;
      end if;
    end if;
  end Correct;