lumatrix.c++

面向对象的卡尔曼滤波器源码

// LUMatrix.cpp		a LU matrix class implementation

//                      (c) Copyright 1995, Everett F. Carter Jr.
//                      Permission is granted by the author to use
//			this software for any application provided this
//			copyright notice is preserved.

static const char rcsid[] = "@(#)lumatrix.c++	1.11 12:38:40 5/13/96   EFC";

// #define DEBUG

#include <stdio.h>

#include <lumatrix.hpp>

LUMatrix::LUMatrix(const LUMatrix &a)	// LUMatrix m = a
{
	int i, j;
	int r = a.rows,	c = a.cols;

	rz = 0;	   // prevents resize() from throwing away space that was never
		   // allocated

	resize(r,c);
	pivot = new int[r];
	
	for (i = 0; i < r; i++)
	{
		pivot[i] = a.pivot[i];
		for (j = 0; j < c; j++)
			m[i][j] = a.m[i][j];
	}
	
}


LUMatrix& LUMatrix::operator=(const LUMatrix &a)		// m = a
{
	int i, j, r = a.rows, c = a.cols;

	if ( this == &a )
		return *this;

	resize(r,c);

	if ( pivot )
		delete []pivot;

	pivot = new int[r];

	for (i = 0; i < r; i++)
	{
		pivot[i] = a.pivot[i];
		for (j = 0; j < c; j++)
			m[i][j] = a.m[i][j];
	}

	return *this;
}

/* lufact.c	lufact		*/

/* routines for linear systems processing via LU factorization
	from C Tools for Scientist and Engineers, L. Baker, 1989	*/

LUMatrix lufact(const Matrix &a)
{
	int i, j, k, l, kp1, nm1, n;
	int *piv;
	double t, q;

	n = a.rows;

	LUMatrix lu(n,n);

	// lu = a;     must do it by copying or LUFACT will be recursively called !
	for (i = 0; i < n; i++)
			for (j = 0; j < n; j++)
				lu[i][j] = a[i][j];

	lu.errval = 0;
	nm1 = n - 1;
	piv = lu.pivot;
	for (k = 0; k < n; k++)
			*(piv++) = k;

	if (nm1 >= 1)	/* non-trivial problem */
	{
		for (k = 0; k < nm1; k++)
		{
			kp1 = k + 1;
			/* partial pivoting ROW exchanges
				-- search over column */
			t = 0.0;
			l = k;
			for (i = k; i < n; i++)
			{
				q = fabs( lu.m[i][k] );
				if ( q > t )
				{
					t = q;
					l = i;
				}
			}
			lu.pivot[k] = l;

			if ( lu.m[l][k] != 0.0 )
			{			/* nonsingular pivot found */
				if (l != k )	/* interchange needed */
					for (i = 0; i < n; i++)
					{
						t = lu.m[l][i];
						lu.m[l][i] = lu.m[k][i];
						lu.m[k][i] = t;
					}
				q = 1.0 / lu.m[k][k];	/* scale row */
				for (i = kp1; i < n; i++)
				{
					t = q * lu.m[i][k];
					lu.m[i][k] = t;
					for (j = kp1; j < n; j++)
						lu.m[i][j] -= t * lu.m[k][j];
				}
			}
			else		/* pivot singular */
				lu.errval = k;
		}

	}

	lu.pivot[ nm1 ] = nm1;
	if (lu.m[nm1][nm1] == 0.0)
		lu.errval = nm1;

	return lu;

}

#ifndef __GNUC__
// a type cast from an LUMatrix directly to a Matrix
LUMatrix::operator Matrix() const		// Matrix m = LUMatrix a
{
	int i, j, r = rows, c = cols;


	Matrix mat( r, c );
	
	for (i = 0; i < r; i++)
		for (j = 0; j < c; j++)
			mat[i][j] = m[i][j];

	return mat;
	
}
#endif

Matrix LUMatrix::L() const
{
	int i, j;
	Matrix lm( rows, cols );

	for (i = 0; i < rows; i++)
		for (j = 0; j <= i; j++)
		{
			if ( j >= cols )
				break;

			if ( i == j )
				lm[i][j] = 1.0;
			else
				lm[i][j] = m[i][j];
		}

	return lm;
}

Matrix LUMatrix::U() const
{
	int i, j;
	Matrix lm( rows, cols );

	for (i = 0; i < rows; i++)
		for (j = i; j < cols; j++)
		{
			if ( j >= cols )
				break;

			lm[i][j] = m[i][j];
		}

	return lm;
}


void LUMatrix::column_swap(Matrix& p, const int k1, const int k2) const
{
	float v;

	if ( k1 == k2 )
		return;

	for (int i = 0; i < p.rows; i++)
	{
		v = p[i][k1];
		p[i][k1] = p[i][k2];
		p[i][k2] = v;
	}

}

Matrix LUMatrix::P() const
{
	Matrix p( rows, cols );

	p = 1.0;

	for (int i = 0; i < cols; i++)
		if ( pivot[i] != i )
			column_swap( p, i, pivot[i] );

	return p;	
}