From mboxrd@z Thu Jan  1 00:00:00 1970
X-Spam-Checker-Version: SpamAssassin 3.4.4 (2020-01-24) on polar.synack.me
X-Spam-Level: 
X-Spam-Status: No, score=-1.9 required=5.0 tests=BAYES_00 autolearn=ham
	autolearn_force=no version=3.4.4
X-Google-Language: ENGLISH,ASCII-7-bit
X-Google-Thread: 103376,83d2d63f98e99c58,start
X-Google-Attributes: gid103376,public
X-Google-ArrivalTime: 2003-01-13 13:22:05 PST
Path: archiver1.google.com!postnews1.google.com!not-for-mail
From: ma740988@pegasus.cc.ucf.edu (Mark)
Newsgroups: comp.lang.ada
Subject: C to Ada conversion
Date: 13 Jan 2003 13:22:04 -0800
Organization: http://groups.google.com/
Message-ID: <a5ae824.0301131322.2c11d495@posting.google.com>
NNTP-Posting-Host: 192.91.147.35
Content-Type: text/plain; charset=ISO-8859-1
Content-Transfer-Encoding: 8bit
X-Trace: posting.google.com 1042492924 18591 127.0.0.1 (13 Jan 2003 21:22:04
 GMT)
X-Complaints-To: groups-abuse@google.com
NNTP-Posting-Date: 13 Jan 2003 21:22:04 GMT
Xref: archiver1.google.com comp.lang.ada:32978
Date: 2003-01-13T21:22:04+00:00
List-Id: <comp.lang.ada>

Interested in doing some benchmarking in Ada.  Trouble is the I need
to convert the C code below to Ada and my Ada is well...
I'd greatly appreaciate it if someone could 'convert' the function
referenced below.  I'm also curious on how Ada allocates and
deallocates objects

Thanks 


--------------------------------------------------------------------

#define ns 30
#define ms 12
#define rs 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
float CPCTSz[rs][rs]; // This is the matrix C*P*CT+Sz
float CPCTSzInv[rs][rs]; // This is the matrix (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


int MatrixInversion(float* A, int n, float* AInverse)
{
  // A = input matrix (n x n)
  // n = dimension of A 
  // AInverse = inverted matrix (n x n)
  // This function inverts a matrix based on the Gauss Jordan method.
  // The function returns 1 on success, 0 on failure.
  int i, j, iPass, imx, icol, irow;
  float det, temp, pivot, factor;
  float* ac = (float*)calloc(n*n, sizeof(float));
  det = 1;
  for (i = 0; i < n; i++)
	{
	for (j = 0; j < n; j++)
		{
		AInverse[n*i+j] = 0;
		ac[n*i+j] = A[n*i+j];
		}
	AInverse[n*i+i] = 1;
	}
  // The current pivot row is iPass.  

  // For each pass, first find the maximum element in the pivot
column.
  for (iPass = 0; iPass < n; iPass++)
	{
	imx = iPass;
	for (irow = iPass; irow < n; irow++)
		{
		if (fabs(A[n*irow+iPass]) > fabs(A[n*imx+iPass])) imx = irow;
		}
	// Interchange the elements of row iPass and row imx in both A and
AInverse.
	if (imx != iPass)
		{
		for (icol = 0; icol < n; icol++)
			{
			temp = AInverse[n*iPass+icol];
			AInverse[n*iPass+icol] = AInverse[n*imx+icol];
			AInverse[n*imx+icol] = temp;
			if (icol >= iPass)
				{
				temp = A[n*iPass+icol];
				A[n*iPass+icol] = A[n*imx+icol];
				A[n*imx+icol] = temp;
				}
			}
		}
	// The current pivot is now A[iPass][iPass].
	// The determinant is the product of the pivot elements.
	pivot = A[n*iPass+iPass];
	det = det * pivot;
	if (det == 0) 
		{
		free(ac);
		return 0;
		}
	  for (icol = 0; icol < n; icol++)
		{
		// Normalize the pivot row by dividing by the pivot element.
		AInverse[n*iPass+icol] = AInverse[n*iPass+icol] / pivot;
		if (icol >= iPass) A[n*iPass+icol] = A[n*iPass+icol] / pivot;
		}
	  for (irow = 0; irow < n; irow++)
		// 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.
		{
		if (irow != iPass) factor = A[n*irow+iPass];
		for (icol = 0; icol < n; icol++)
			{
			if (irow != iPass)
				{
				AInverse[n*irow+icol] -= factor * AInverse[n*iPass+icol];
				A[n*irow+icol] -= factor * A[n*iPass+icol];
				}
			}
		}
	}
	free(ac);
	return 1;
}

int main (void)
{

  int n = ns;
  int m = ms;
  int r = rs;

  MatrixInversion((float*)CPCTSz, r, (float*)CPCTSzInv);
  return 0;
}