Re: C to Ada conversion

[tmoran@acm.org]

> Interested in doing some benchmarking in Ada.  ... convert
  See below
> I'm also curious on how Ada allocates and deallocates objects
  There is an alloc(new)/free mechanism but usually it's more efficient,
as well as easier and less error prone, to just let the compiler allocate
things stack-wise.  See matrix "Ac" below.  It's allocated automatically,
and deallocated automatically.  (Actually, since it's never used here,
one hopes a good compiler doesn't bother generating any code for it.)

procedure Inv is

  type Matrices is array (Integer range <>, Integer range <>) of Float;

  procedure Matrix_Inversion(A       : in out Matrices;
                             Inverse :    out Matrices) is
  -- This function inverts a matrix based on the Gauss Jordan method.
  -- It raises Constraint_Error if A or Inverse is not square
  -- or if their index ranges don't match
  -- or if the determinant of A is too small.
  -- Note that input matrix A will be modified.
    Det     : Float := 1.0;
    Temp, Pivot, Factor  : Float;
    Ac      : Matrices := A; -- Note: never used, but in C code
  begin

    Inverse := (others => (others => 0.0));
    for I in Inverse'range (1) loop
      Inverse(I, I) := 1.0;
    end loop;

    for iPass in A'range (1) loop
      -- For each pass, first find maximum element in the pivot column.
      declare
        Max_Row : Integer := iPass;
      begin
        for Row in iPass .. A'Last(1) loop
          if abs(A(Row, iPass)) > abs(A(Max_Row, iPass)) then
            Max_Row := Row;
          end if;
        end loop;
        -- Interchange the elements of row iPass and Max_Row in both A and Inverse.
        if Max_Row /= iPass then
          for Col in A'range (2) loop
            Temp := Inverse(iPass, Col);
            Inverse(iPass, Col) := Inverse(Max_Row, Col);
            Inverse(Max_Row, Col) := Temp;
          end loop;
        end if;
      end;

      -- The current pivot is now A(iPass, iPass).
      -- The determinant is the product of the pivot elements.
      Pivot := A(iPass, iPass);
      Det := Det * Pivot;

      if Det = 0.0 then          -- A pointless test in Ada since an
        raise Constraint_Error;  -- attempted divide by zero will raise
      end if;                    -- Constraint_Error anyway.
                                 -- An erroneous test in C since a very
                                 -- small, but non-zero, Det will not be
                                 -- caught, but will cause divide by zero.

      -- Normalize the pivot row by dividing by the pivot element.
      for Col in A'range (2) loop
        Inverse(iPass, Col) := Inverse(iPass, Col) / Pivot;
        if Col >= iPass then
          A(iPass, Col) := A(iPass, Col) / Pivot;
        end if;
      end loop;

      -- Add a multiple of the pivot row to each row.  The multiple factor
      -- is chosen so that the element of A on the pivot column is 0.
      for Row in A'range (1) loop
        -- Note: this loop had two "if (irow != iPass)" statements,
        -- one inside and one outside the column loop.  I've combined
        -- them for clarity and efficiency
        if Row /= iPass then
          Factor := A(Row, iPass);
          for Col in A'range (2) loop
            if Row /= iPass then
              Inverse(Row, Col) := Inverse(Row, Col) - Factor * A(iPass, Col);
              A(Row, Col) := A(Row, Col) - Factor * A(iPass, Col);
            end if;
          end loop;
        end if;
      end loop;

    end loop; -- iPass

  end Matrix_Inversion;

  -- #define ns 30
  -- #define ms 12
  Rs : constant := 6;
  -- A is an n by n matrix, A is the state transistion matrix
  -- B is an n by m matrix, B is the input matrix
  -- C is an r by n matrix, C is the measurement matrix
  -- xhat is an n by 1 vector, xhat is the current state of the system
  -- y is an r by 1 vector, y is the measured output of the system
  -- u is an m by 1 vector, u is the known input to the system
  -- Sz is an r by r matrix, Sz is the measurement nise covariance
  -- Sw is an n by n matrix, Sw is the process noise covariance
  -- P is an n by n matrix, P is the estimator error covariance
  -- float A[ns][ns];
  -- float B[ns][ms];
  -- float C[rs][ns];
  -- float xhat[ns][1];
  -- float y[rs][1];
  -- float u[ms][1];
  -- float Sz[rs][rs];
  -- float Sw[ns][ns];
  -- float P[ns][ns];
  -- float AP[ns][ns]; // This is the matrix A*P
  -- float CT[ns][rs]; // This is the matrix CT
  -- float APCT[ns][rs]; // This is the matrix A*P*CT
  -- float CP[rs][ns]; // This is the matrix C*P
  -- float CPCT[rs][rs]; // This is the matrix C*P*CT
  CPCTSz  : Matrices(1 .. Rs, 1 .. Rs); -- This is the matrix C*P*CT+Sz
  CPCTSzInv: Matrices(1 .. Rs, 1 .. Rs); -- (C*P*CT+Sz)**-1
  -- float K[ns][rs]; // This is the Kalman gain.
  -- float Cxhat[rs][1]; // This is the vector C*xhat
  -- float yCxhat[rs][1]; // This is the vector y-C*xhat
  -- float KyCxhat[ns][1]; // This is the vector K*(y-C*xhat)
  -- float Axhat[ns][1]; // This is the vector A*xhat
  -- float Bu[ns][1]; // This is the vector B*u
  -- float AxhatBu[ns][1]; // This is the vector A*xhat+B*u
  -- float AT[ns][ns]; // This is the matrix AT
  -- float APAT[ns][ns]; // This is the matrix A*P*AT
  -- float APATSw[ns][ns]; // This is the matrix A*P*AT+Sw
  -- float CPAT[rs][ns]; // This is the matrix C*P*AT
  -- float SzInv[rs][rs]; // This is the matrix Sz-1
  -- float APCTSzInv[ns][rs]; // This is the matrix A*P*CT*Sz-1
  -- float APCTSzInvCPAT[ns][ns]; // This is the matrix A*P*CT*Sz-1*C*P*AT

begin
  Matrix_Inversion(CPCTSz, CPCTSzInv);
end Inv;