matrix.cpp

VC++实现的kalman代码

// Matrix.cpp: implementation of the CMatrix class.
//
//////////////////////////////////////////////////////////////////////



#include "stdafx.h"
#include "Matrix.h"
#include <iostream.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////





CMatrix::CMatrix(int row)//重载构造函数
{
int i,j;

  m_mat=new double *[row];

	for(i=0;i<row;i++)
	 m_mat[i]=new double [row];
	for(i=0;i<row;i++)
		for(j=0;j<row;j++)
			m_mat[i][j]=0;
	m_row=row;
	m_col=row;
	return;



}


CMatrix::CMatrix(int row ,int col)//重载构造函数
{
	int i,j;

	m_mat=new double *[row];

	for(i=0;i<row;i++)
	 m_mat[i]=new double [col];
	for(i=0;i<row;i++)
		for(j=0;j<col;j++)
			m_mat[i][j]=0;
	m_row=row;
	m_col=col;
	return;


}

CMatrix::CMatrix(const CMatrix &temp)//显式复制构造函数
{
	m_row=temp.m_row;
	m_col=temp.m_col;

		int i,j;

	m_mat=new double *[m_row];

	for(i=0;i<m_row;i++)
	 m_mat[i]=new double [m_col];
	for(i=0;i<m_row;i++)
		for(j=0;j<m_col;j++)
			m_mat[i][j]=temp.m_mat[i][j];

}

void CMatrix::SetSize(int row,int col)//设置矩阵大小
{
	int i,j;

	m_mat=new double *[row];

	for(i=0;i<row;i++)
	 m_mat[i]=new double [col];
	for(i=0;i<row;i++)
		for(j=0;j<col;j++)
			m_mat[i][j]=0;
	m_row=row;
	m_col=col;
	return;


}
void CMatrix::SetSize(int row)//设置矩阵大小
{
	int i,j;

	 m_mat=new double *[row];

	for(i=0;i<row;i++)
	 m_mat[i]=new double [row];
	for(i=0;i<row;i++)
		for(j=0;j<row;j++)
			m_mat[i][j]=0;
	m_row=row;
	m_col=row;
	return;


}

CMatrix::~CMatrix()//析构函数
{

	int i;
	for(i=0;i<m_row;i++)
		delete []m_mat[i];
	delete []m_mat;

 //	delete []m_mat;


}

//Dolittle分解求矩阵逆

void CMatrix::MatInverse(double **mat, int dim)
{
 int i,j,*indx;
 double d,*col;

 double **tempmat=new double*[dim];
 for(i=0;i<dim;i++)
	 tempmat[i]=new double [dim];
		
 indx = (int *)malloc((unsigned)(dim*sizeof(int)));
 
 double *tempvector=new double[dim];
 col=tempvector;

 MatLudcmp(mat,dim,indx,&d);
 for (j=0;j<dim;j++)
    {
     for (i=0;i<dim;i++) col[i] = 0.0;
     col[j] = 1.0;
     MatLubksb(mat,dim,indx,col);
     for (i=0;i<dim;i++) tempmat[i][j]= col[i];
    }
 for (i=0;i<dim;i++)
    for (j=0;j<dim;j++)
       mat[i][j] = tempmat[i][j];

 delete []tempvector;
 	for(i=0;i<dim;i++)
		delete []tempmat[i];
	delete []tempmat;
//	delete []tempvector;
 free(indx);
}

void CMatrix::MatLudcmp(double **a, int n, int *indx, double *d)
{
 int i,imax,j,k;
 double   big,dum,sum,temp;
 double   *vv;

 vv = (double*)malloc((unsigned)(n*sizeof(double)));
 if (!vv) 
   {
    fprintf(stderr,"Error Allocating Vector Memory\n");
    exit(1);
   }
 *d = 1.0;
 for (i=0;i<n;i++)
    {
     big = 0.0;
     for (j=0;j<n;j++)
     {
      if ((temp=fabs(a[i][j])) > big) big = temp;
     }
     if (big == 0.0)
       {
        fprintf(stderr,"Singular Matrix in Routine LUDCMP\n");
     for (j=0;j<n;j++) printf(" %f ",a[i][j]); printf("/n");
     exit(1);
       }
     vv[i] = 1.0/big;
    }
 for (j=0;j<n;j++)
    {
     for (i=0;i<j;i++)
     {
      sum = a[i][j];
      for (k=0;k<i;k++) sum -= a[i][k] * a[k][j];
      a[i][j] = sum;
     }
     big = 0.0;
     for (i=j;i<n;i++)
        {
      sum = a[i][j];
      for (k=0;k<j;k++) sum -= a[i][k] * a[k][j];
      a[i][j] = sum;
      if ((dum=vv[i]*fabs(sum)) >= big)
        {
         big = dum;
         imax = i;
        }
     }
     if (j != imax)
       {
     for (k=0;k<n;k++)
        {
         dum = a[imax][k];
         a[imax][k] = a[j][k];
         a[j][k] = dum;
        }
        *d = -(*d);
     vv[imax] = vv[j];
       }
     indx[j] = imax;
     if (a[j][j] == 0.0) a[j][j] = TINY;
     if (j != n-1)
       {
     dum = 1.0 / a[j][j];
     for (i=j+1;i<n;i++) a[i][j] *= dum;
       }
    }
 free(vv);
}

void CMatrix::MatLubksb(double **a, int n, int *indx, double *b)
{
 int i,ip,j,ii=-1;
 double   sum;

 for (i=0;i<n;i++)
    {
     ip = indx[i];
     sum = b[ip];
     b[ip] = b[i];
     if (ii>=0)
       for (j=ii;j<i;j++) sum -= a[i][j] * b[j];
     else if (sum) ii = i;
     b[i] = sum;
    }
 for (i=n-1;i>=0;i--)
    {
     sum = b[i];
     for (j=i+1;j<n;j++) sum -= a[i][j] * b[j];
     b[i] = sum / a[i][i];
    }
}

	//操作符重载

CMatrix CMatrix::operator +(CMatrix &a)//矩阵加法
{
    int i,j;

	if(a.m_col!=m_col)
 		exit(1);
 	if(a.m_row!=m_row)
 		exit(1);
	CMatrix temp(m_row,m_col);
	for(i=0;i<m_row;i++)
		for(j=0;j<m_col;j++)
			temp.m_mat[i][j]=a.m_mat[i][j]+m_mat[i][j];
	return temp;


}

CMatrix CMatrix::operator -(CMatrix &a)//矩阵减法
{
    int i,j;

	if(a.m_col!=m_col)
 		exit(1);
 	if(a.m_row!=m_row)
 		exit(1);
	CMatrix temp(m_row,m_col);
	for(i=0;i<m_row;i++)
		for(j=0;j<m_col;j++)
			temp.m_mat[i][j]=m_mat[i][j]-a.m_mat[i][j];
	return temp;


}

CMatrix CMatrix::operator *(CMatrix &a)//矩阵乘法
{
    int i,j;

	if(m_col!=a.m_row)
 		exit(1);
 	CMatrix temp(m_row,a.m_col);
  	for (i=0;i<m_row;i++)
 		for(j=0;j<a.m_col;j++)
 		{
 			for(int n=0;n<m_col;n++)
 				temp.m_mat[i][j]+=m_mat[i][n]*a.m_mat[n][j];
 				
 		}
 
	return temp;


}

void CMatrix::Display()//矩阵显示
{
		int i,j;
	 for( j=0;j<m_row;j++)
		{
			for( i=0;i<m_col;i++)
			cout<<m_mat[j][i]<<"    ";
			cout<<endl;
		}
	 cout<<endl;

}

CMatrix& CMatrix::operator =(CMatrix &a)//矩阵赋值
{
	int i,j;
	if(a.m_col!=m_col)
 		exit(1) ;
 	if(a.m_row!=m_row)
 		exit(1);
	for(i=0;i<m_row;i++)
		for(j=0;j<m_col;j++)
			m_mat[i][j]=a.m_mat[i][j];
	return *this;
}

double& CMatrix::operator()(int row,int col)//取矩阵元素
{

	if(row<1||row>m_row||col<1||col>m_col)
 		exit(1) ;
	return m_mat[row-1][col-1];
}

CMatrix CMatrix::operator !()//矩阵求逆
{
    int i,j;

	if(m_col!=m_row)
 		exit(1);
	CMatrix temp(m_row);
	for(i=0;i<m_row;i++)
		for(j=0;j<m_col;j++)
			temp.m_mat[i][j]=m_mat[i][j];

	MatInverse(temp.m_mat,m_row);

	return temp;


}
CMatrix CMatrix::operator ~()//矩阵转置
{
    int i,j;

	CMatrix temp(m_col,m_row);
	for(i=0;i<m_row;i++)
		for(j=0;j<m_col;j++)
			temp.m_mat[j][i]=m_mat[i][j];

	return temp;


}

CMatrix CMatrix::operator *(double d)//矩阵与数相乘
{
	int i,j;
	CMatrix temp(m_row,m_col);
	for(i=0;i<m_row;i++)
		for(j=0;j<m_col;j++)
			temp.m_mat[j][i]=m_mat[i][j]*d;
	return temp;

}