matrix.c++

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

// Matrix.cpp		a matrix class implementation
//	nonsquare generalization of Computers in Physics Jan/Feb 1990

//                      (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[] = "@(#)matrix.c++	1.7 12:26:13 4/24/95   EFC";

// #define DEBUG

#include <error.hpp>
#include <matrix.hpp>

Matrix& Matrix::operator=(const Matrix &a)		// m = a
{
	int i, j;

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

	if ( a.rows != rows || a.cols != cols )
	{
		Error error("Matrix =, Matrices different sizes");
		if ( rows != a.rows )
		error << " rows = " << rows << ", a.rows = " << a.rows;
		if ( cols != a.cols)
		error << " cols = " << cols << ", a.cols = " << a.cols;
		error.fatal();
	}

	for (i = 0; i < rows; i++)
		for (j = 0; j < cols; j++)
			m[i][j] = a.m[i][j];

	by_columns = a.by_columns;

	return *this;

}

void Matrix::diag(const scalar_type a)		// diagonal of m = scalar(a)
{
	int i, iend;

	iend = rows;
	if ( iend > cols )
		iend = cols;

	for (i = 0; i < iend; i++)
			m[i][i] = a;

}

Matrix operator+(const Matrix &a,const Matrix &b)		//  m = a + b
{
	if ( a.rows != b.rows || a.cols != b.cols )
	{
		Error error("Matrix +, matrices different sizes");
		if ( a.rows != b.rows )
		error << " a.row = " << a.rows << ", b.row = " << b.rows;
		if ( a.cols != b.cols)
		error << " a.col = " << a.cols << ", b.col = " << b.cols;
		error.fatal();
	}

	int i, j, row = a.rows, col = a.cols;
#ifdef DEBUG
	cerr << "Matrix sum will be " << row << " by " << col << endl;
	cerr << "Sum using matrices:\n" << a << '\n' << b << endl;
#endif
	Matrix sum(row,col);

	for (i = 0; i < row; i++)
		for (j = 0; j < col; j++)
			sum.m[i][j] = a.m[i][j] + b.m[i][j];

#ifdef DEBUG
	cerr << "Sum is:\n" << sum << endl;
#endif

	return sum;

}

Matrix operator-(const Matrix &a,const Matrix &b)		//  m = a - b
{
	int i, j;

	if ( a.rows != b.rows || a.cols != b.cols )
	{
		Error error("Matrix -, matrices different sizes");
		if ( a.rows != b.rows )
		error << " a.row = " << a.rows << ", b.row = " << b.rows;
		if ( a.cols != b.cols)
		error << " a.col = " << a.cols << ", b.col = " << b.cols;
		error.fatal();
	}

	int row = a.rows, col = a.cols;
	Matrix diff(row,col);

	for (i = 0; i < row; i++)
		for (j = 0; j < col; j++)
			diff.m[i][j] = a.m[i][j] - b.m[i][j];

	return diff;

}

Matrix operator*(const Matrix &a,const Matrix &b)		//  m = a * b
{
	if ( a.cols != b.rows )
	{
		Error error("Matrix *");
		error << "Matrix a * Matrix b incommensurate, a.cols = " << a.cols
			<< " , b.rows = " << b.rows;
		error.fatal();
	}

	int i, j, k, row = a.rows, col = b.cols, size = a.cols;
	Matrix prod(row,col);
	scalar_type zero = 0.0;


	for (i = 0; i < row; i++)
		for (j = 0; j < col; j++)
			for (k = 0, prod.m[i][j] = zero; k < size; k++)
				prod.m[i][j] += a.m[i][k] * b.m[k][j];

	return prod;

}

Matrix  operator*(const double &d,const Matrix &a)		// m = d * a
{
	int i, j, row = a.rows, col = a.cols;
	Matrix b(row,col);

	for (i = 0; i < row; i++)
		for (j = 0; j < col; j++)
			b.m[i][j] = d * a.m[i][j];

	return b;

}


#ifdef COMPLEX

Matrix  operator*(const Complex &c,const Matrix &a)		// m = c * a
{
	int i, j, row = a.rows, col = a.cols;
	Matrix b(row,col);

	for (i = 0; i < row; i++)
		for (j = 0; j < col; j++)
			b.m[i][j] = c * a.m[i][j];

	return b;

}

#endif


#ifdef INCLUDE_VECTOR

Vector operator*(const Matrix &a, Vector &x)		//  v = a * x
{
	if ( a.cols != x.size() )
	{
		Error error("Matrix * Vector");
		error << "Matrix a * Vector x incommensurate, a.cols = " << a.cols
				<< ", x.rows = " << x.size();
		error.fatal();
	}

	int i, k, row = a.rows, size = a.cols;
	Vector prod(row);
	scalar_type zero = 0.0;


	for (i = 0; i < row; i++)
		for (k = 0, prod[i] = zero; k < size; k++)
				prod[i] += (scalar_type)(x[k]) * a.m[i][k];

	return prod;

}

#endif

int operator==(const Matrix &a,const Matrix &b)	//	int i = (a == b)
{
	if ( a.rows != b.rows || a.cols != b.cols )
	{
		Error error("Matrix ==");
		if ( a.rows != b.rows )
		error << "Matrix a and Matrix b different sizes  a.row = " << a.rows
			<< ", b.row = " << b.rows;
		if ( a.cols != b.cols)
		error << "Matrix a and Matrix b different sizes, a.col = " << a.cols
			<< ", b.col = " << b.cols;
		error.fatal();
	}

	int i, j, row = a.rows, col = a.cols;
	int l = 1;

	for (i = 0; i < row; i++)
		for (j = 0; j < col; j++)
			l = l && ( a.m[i][j] == b.m[i][j] );

	return l;

}

Matrix comm(const Matrix &a,const Matrix &b)	// m = comm(a,b)
{
	Matrix r = a * b - b * a;

	return r;
}

scalar_type trace(const Matrix &a)		// m = trace( a )
{
	int size = a.rows;
	scalar_type sum = 0.0;

	if ( size > a.cols )
		size = a.cols;

	for (int d = 0; d < size; d++)
			sum += a.m[d][d];

	return sum;

}

Matrix herm(const Matrix &a)		// m = hermitian( a )
{
	int i, j, r = a.cols, c = a.rows;
	Matrix adj(r,c);

	for (i = 0; i < r; i++)
		for (j = 0; j < c; j++)
#ifdef COMPLEX
			adj.m[i][j] = conj(a.m[j][i]);
#else
			adj.m[i][j] = a.m[j][i];
#endif

	return adj;

}

Matrix transpose(const Matrix &a)		// m = transpose( a )
{					// same as hermitian for real matrix
	int i, j, r = a.cols, c = a.rows;
	Matrix adj(r,c);

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


	return adj;

}