leastmeansquare.cpp

最小二乘法求超定线性方程组得C＋＋模板

/************************************************************************/
/*  A*H=B */
/************************************************************************/
template <class T>
bool LeastMeanSquare(T* A, T* B, int nRows, int nCols, T* H ){
	T* AT=new T [nRows*nCols];
	T* ATA=new T [nCols*nCols];
	T* Q=new T [nRows*nCols];
	brmul(AT , A, nCols, nRows, nCols, ATA);
	if(!brinv(ATA, nCols)){
                   return 0;
                   }
	
	brmul(ATA , AT, nCols, nCols, nRows, Q);
	brmul(Q , B, nCols, nRows, nCols, H);
	
	return 1;
	

}

/********************************************************
*  brinv
*
*  PARAMETERS:
*  double *a		-The n*n Matrix 
*  int n			-The dimention of matrix a[]
*
*  DESCRIPTION:
*  Caculate the inverted matrix of *a, then put the value to *a.
*********************************************************/
template <class T>
int brinv(T *a, int n) 
{ 
	int *is,*js,i,j,k,l,u,v; 
	T d,p; 

	is=new int[n];
	js=new int[n];

	for (k=0; k<=n-1; k++) 
	{ 
		d=0.0; 
		for (i=k; i<=n-1; i++) 
			for (j=k; j<=n-1; j++) 
				{ 
					l=i*n+j; 
					p=fabs(a[l]); 
					if (p>d) 
					{ 
						d=p; 
						is[k]=i; 
						js[k]=j;
					} 
				} 

		if (d+1.0==1.0) 
		{ 
			delete [] is;
			delete [] js;
			printf("err**not inv\n"); 
			return 0; 
		} 

		if (is[k]!=k) 
			for (j=0; j<=n-1; j++) 
			{ 
				u=k*n+j; 
				v=is[k]*n+j; 
				p=a[u]; 
				a[u]=a[v]; 
				a[v]=p; 
			} 

		if (js[k]!=k) 
			for (i=0; i<=n-1; i++) 
			{ 
				u=i*n+k; 
				v=i*n+js[k]; 
				p=a[u]; 
				a[u]=a[v]; 
				a[v]=p; 
			} 

		l=k*n+k; 
		a[l]=1.0/a[l]; 

		for (j=0; j<=n-1; j++) 
			if (j!=k) 
			{ 
				u=k*n+j; 
				a[u]=a[u]*a[l];
			} 

		for (i=0; i<=n-1; i++) 
			if (i!=k) 
				for (j=0; j<=n-1; j++) 
					if (j!=k) 
					{ 
						u=i*n+j; 
						a[u]=a[u]-a[i*n+k]*a[k*n+j]; 
					} 

		for (i=0; i<=n-1; i++) 
			if (i!=k) 
			{ 
				u=i*n+k; 
				a[u]=-a[u]*a[l];
			} 
	} 

	for (k=n-1; k>=0; k--) 
	{ 
		if (js[k]!=k) 
			for (j=0; j<=n-1; j++) 
			{ 
				u=k*n+j; 
				v=js[k]*n+j; 
				p=a[u]; 
				a[u]=a[v]; 
				a[v]=p; 
			} 
		if (is[k]!=k) 
			for (i=0; i<=n-1; i++) 
			{ 
				u=i*n+k; 
				v=i*n+is[k]; 
				p=a[u]; 
				a[u]=a[v]; 
				a[v]=p; 
			} 
	} 

	delete [] is;
	delete [] js;
	js=NULL;
	is=NULL;

	return 1; 
} 

/********************************************************
*  brmul
*
*  PARAMETERS:
*  double *a		-The m*n Matrix 
*  double *b		-The n*k Matrix 
*  double *c		-The result of a[]*b[] 
*
*  DESCRIPTION:
*  Caculate the multiplication of matrix of a[]*b[]
*********************************************************/
template <class T>
void brmul(T *a, T *b,int m,int n,int k, T *c) 
{ 
	int i,j,l,u; 
	for (i=0; i<=m-1; i++) 
		for (j=0; j<=k-1; j++) 
		{ 
			u=i*k+j; 
			c[u]=0.0; 
			for (l=0; l<=n-1; l++) 
				c[u]=c[u]+a[i*n+l]*b[l*k+j]; 
		} 
	return; 
} 

/********************************************************
*  brtranspos
*
*  PARAMETERS:
*  double *a		-The m*n Matrix 
*  double *b		-The transposition of a 
*
*  DESCRIPTION:
*  Caculate the transposition of matrix of a.
*********************************************************/
template <class T>
void brtranspos(T *a, int m,int n, T *b) 
{ 
	int i,j; 
	for (i=0; i<=m-1; i++) 
	{
		for (j=0; j<=n-1; j++) 
		{
			b[j*m+i]=a[i*n+j];
		} 
	}
	return; 
}