SLAM.Inertial.SchmidtGraphPoseCorrection.Solve

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

function Solve
    input Real A[12,12]; input Real B[12,22];
    output Real X[12,22]; output Boolean valid;
  protected
    Real scale[12]; Real lower[12,12]; Real forward[12]; Real backward[12];
    Real value; Real residual; Real magnitude;
  algorithm
    X := zeros(12,22); lower := zeros(12,12); scale := ones(12);
    forward := zeros(12); backward := zeros(12); valid := true;
    for i in 1:12 loop
      valid := valid and A[i,i] > 0 and A[i,i] <= 1e12;
      if A[i,i] > 0 and A[i,i] <= 1e12 then scale[i] := sqrt(A[i,i]); end if;
      for j in 1:12 loop
        valid := valid and abs(A[i,j]) <= 1e12
          and abs(A[i,j]-A[j,i]) <= 1e-12*max(1.0,abs(A[i,j])+abs(A[j,i]));
      end for;
      for rhs in 1:22 loop valid := valid and abs(B[i,rhs]) <= 1e12; end for;
    end for;
    if valid then
      for i in 1:12 loop
        for j in 1:i loop
          value := A[i,j]/scale[i]/scale[j];
          for k in 1:j-1 loop value := value-lower[i,k]*lower[j,k]; end for;
          if j == i then
            valid := valid and value > 0 and value <= 1e12;
            lower[i,i] := sqrt(if value > 0 and value <= 1e12 then value else 1.0);
          else
            lower[i,j] := value/lower[j,j];
          end if;
        end for;
      end for;
    end if;
    if valid then
      for rhs in 1:22 loop
        for i in 1:12 loop
          value := B[i,rhs]/scale[i];
          for j in 1:i-1 loop value := value-lower[i,j]*forward[j]; end for;
          forward[i] := value/lower[i,i];
        end for;
        for reverse in 1:12 loop
          value := forward[13-reverse];
          for j in 14-reverse:12 loop value := value-lower[j,13-reverse]*backward[j]; end for;
          backward[13-reverse] := value/lower[13-reverse,13-reverse];
        end for;
        for i in 1:12 loop X[i,rhs] := backward[i]/scale[i]; end for;
      end for;
      for i in 1:12 loop for rhs in 1:22 loop
        residual := -B[i,rhs]; magnitude := abs(B[i,rhs]);
        for j in 1:12 loop
          residual := residual+A[i,j]*X[j,rhs]; magnitude := magnitude+abs(A[i,j]*X[j,rhs]);
        end for;
        valid := valid and abs(X[i,rhs]) <= 1e12 and abs(residual) <= 1e-9*max(1.0,magnitude);
      end for; end for;
    end if;
    if not valid then X := zeros(12,22); end if;
  end Solve;