matrix.cpp

《陈必红算法》一书的附带的源代码

	return m;
}

DOUBLE matrix::det(DOUBLE err)		// 求行列式的值
{
	if(rownum != colnum || rownum == 0)
		throw TMESSAGE("Can not calculate det");
	matrix m(*this);
	size_t rank;
	return m.detandrank(rank, err);
}

size_t matrix::rank(DOUBLE err)	// 求矩阵的秩
{
	if(rownum==0 || colnum==0)return 0;
	size_t k;
	k = rownum > colnum ? colnum : rownum;
	matrix m(k,k);		// 产生k阶方阵
	for(size_t i=0; i<k; i++)
	for(size_t j=0; j<k; j++)
		m.set(i,j,value(i,j));
	size_t r;
	m.detandrank(r, err);
	return r;
}

DOUBLE matrix::detandrank(size_t & rank, DOUBLE err)	// 求方阵的行列式和秩
{
	if(rownum != colnum || rownum == 0)
		throw TMESSAGE("calculate det and rank error!");
	size_t i,j,k,is,js;
	double f,detv,q,d;
	f=1.0; detv=1.0;
	rank = 0;
	 for (k=0; k<rownum-1; k++)
		{
			q = maxabs(is, js, k);
			if(q<err) return 0.0;	// 如主元太小，则行列式的值被认为是0
			rank++; // 秩增1
			if(is!=k) { f=-f; swapr(k,is,k); }
			if(js!=k) { f=-f; swapc(k,js,k); }
			q = value(k,k);
			detv *= q;
		  for (i=k+1; i<rownum; i++)
			 {
				d=value(i,k)/q;
				for (j=k+1; j<rownum; j++)
					set(i,j,value(i,j)-d*value(k,j));
			 }
		} // end loop k
	 q = value(rownum-1,rownum-1);
	 if(q != 0.0 ) rank++;
	return f*detv*q;
}

void matrix::checksym()	// 检查和尝试调整矩阵到对称矩阵
{
	issym = 0;	// 先假设矩阵非对称
	if(rownum != colnum) return;	// 行列不等当然不是对称矩阵
	DOUBLE a,b;
	for(size_t i=1; i<rownum; i++) // 从第二行开始检查
	for(size_t j=0; j<i; j++) // 从第一列到第i-1列
	{
		a = value(i,j);
		b = value(j,i);
		if( fabs(a-b) >= defaulterr ) return; // 发现不对称，返回
		if(a!=b)set(i,j,b); // 差别很小就进行微调
	}
	issym = 1;	// 符合对称阵标准
}

void matrix::house(buffer & b, buffer & c)// 用豪斯荷尔德变换将对称阵变为对称三对
											// 角阵，b返回主对角线元素，c返回次对角线元素
{
	if(!issym) throw TMESSAGE("not symatry");
	size_t i,j,k;
	DOUBLE h,f,g,h2,a;
	 for (i=rownum-1; i>0; i--)
		{ h=0.0;
		  if (i>1)
			 for (k=0; k<i; k++)
				{ a = value(i,k); h += a*a;}
		  if (h == 0.0)
			 { c[i] = 0.0;
				if (i==1) c[i] = value(i,i-1);
				b[i] = 0.0;
			 }
		  else
			 { c[i] = sqrt(h);
				a = value(i,i-1);
				if (a > 0.0) c[i] = -c[i];
				h -= a*c[i];
				set(i,i-1,a-c[i]);
				f=0.0;
				for (j=0; j<i; j++)
				  { set(j,i,value(i,j)/h);
					 g=0.0;
					 for (k=0; k<=j; k++)
						g += value(j,k)*value(i,k);
					 if(j<=i-2)
						for (k=j+1; k<i; k++)
							g += value(k,j)*value(i,k);
					 c[j] = g/h;
					 f += g*value(j,i);
				  }
				h2=f/(2*h);
				for (j=0; j<i; j++)
				  { f=value(i,j);
					 g=c[j] - h2*f;
					 c[j] = g;
					 for (k=0; k<=j; k++)
						set(j,k, value(j,k)-f*c[k]-g*value(i,k) );
				  }
				b[i] = h;
			 }
		}
	 for (i=0; i<=rownum-2; i++) c[i] = c[i+1];
	 c[rownum-1] = 0.0;
	 b[0] = 0.0;
	 for (i=0; i<rownum; i++)
		{ if ((b[i]!=0.0)&&(i>=1))
			 for (j=0; j<i; j++)
				{ g=0.0;
				  for (k=0; k<i; k++)
					 g=g+value(i,k)*value(k,j);
				  for (k=0; k<i; k++)
					 set(k,j,value(k,j)-g*value(k,i));
				}
		  b[i] = value(i,i);
		  set(i,i,1.0);
		  if (i>=1)
			 for (j=0; j<=i-1; j++)
				{ set(i,j,0.0);
				  set(j,i,0.0); }
		}
	 return;
}

void matrix::trieigen(buffer& b, buffer& c, size_t l, DOUBLE eps)
						// 计算三对角阵的全部特征值与特征向量
{	size_t i,j,k,m,it;
	double d,f,h,g,p,r,e,s;
	c[rownum-1]=0.0; d=0.0; f=0.0;
	for (j=0; j<rownum; j++)
		{ it=0;
		  h=eps*(fabs(b[j])+fabs(c[j]));
		  if (h>d) d=h;
		  m=j;
		  while ((m<rownum)&&(fabs(c[m])>d)) m+=1;
		  if (m!=j)
			 { do
				  { if (it==l) throw TMESSAGE("fial to calculate eigen value");
					 it += 1;
					 g=b[j];
					 p=(b[j+1]-g)/(2.0*c[j]);
					 r=sqrt(p*p+1.0);
					 if (p>=0.0) b[j]=c[j]/(p+r);
					 else b[j]=c[j]/(p-r);
					 h=g-b[j];
					 for (i=j+1; i<rownum; i++)
						b[i]-=h;
					 f=f+h; p=b[m]; e=1.0; s=0.0;
					 for (i=m-1; i>=j; i--)
						{ g=e*c[i]; h=e*p;
						  if (fabs(p)>=fabs(c[i]))
							 { e=c[i]/p; r=sqrt(e*e+1.0);
								c[i+1]=s*p*r; s=e/r; e=1.0/r;
							 }
						  else
				{ e=p/c[i]; r=sqrt(e*e+1.0);
								c[i+1]=s*c[i]*r;
								s=1.0/r; e=e/r;
							 }
						  p=e*b[i]-s*g;
						  b[i+1]=h+s*(e*g+s*b[i]);
						  for (k=0; k<rownum; k++)
							 {
								h=value(k,i+1);
								set(k,i+1, s*value(k,i)+e*h);;
								set(k,i,e*value(k,i)-s*h);
							 }
						  if(i==0) break;
						}
					 c[j]=s*p; b[j]=e*p;
				  }
				while (fabs(c[j])>d);
			 }
		  b[j]+=f;
		}
	 for (i=0; i<=rownum; i++)
		{ k=i; p=b[i];
		  if (i+1<rownum)
			 { j=i+1;
				while ((j<rownum)&&(b[j]<=p))
				  { k=j; p=b[j]; j++;}
			 }
		  if (k!=i)
			 { b[k]=b[i]; b[i]=p;
				for (j=0; j<rownum; j++)
				  { p=value(j,i);
					 set(j,i,value(j,k));
					 set(j,k,p);
				  }
			 }
		}
}

matrix matrix::eigen(matrix & eigenvalue, DOUBLE eps, size_t l)
	 // 计算对称阵的全部特征向量和特征值
		// 返回按列排放的特征向量，而eigenvalue将返回一维矩阵，为所有特征值
		// 组成的列向量
{
	if(!issym) throw TMESSAGE("it is not symetic matrix");
	eigenvalue = matrix(rownum); // 产生n行1列向量准备放置特征值
	matrix m(*this); // 复制自己产生一新矩阵
	if(rownum == 1) {	// 如果只有1X1矩阵，则特征向量为1，特征值为value(0,0)
		m.set(0,0,1.0);
		eigenvalue.set(0,value(0,0));
		return m;
	}
	buffer * b, *c;
	b = getnewbuffer(rownum);
	c = getnewbuffer(rownum);
	m.house(*b,*c);	// 转换成三对角阵
	m.trieigen(*b,*c,l,eps); // 算出特征向量和特征值
	for(size_t i=0; i<rownum; i++) // 复制b的内容到eigenvalue中
		eigenvalue.set(i,(*b)[i]);
	return m;
}

void matrix::hessenberg()	// 将一般实矩阵约化为赫申伯格矩阵
{
	 size_t i,j,k;
	 double d,t;
	 for (k=1; k<rownum-1; k++)
		{ d=0.0;
		  for (j=k; j<rownum; j++)
			 { t=value(j,k-1);
				if (fabs(t)>fabs(d))
				  { d=t; i=j;}
			 }
		  if (fabs(d)!=0.0)
			 { if (i!=k)
				  { for (j=k-1; j<rownum; j++)
						{
						  t = value(i,j);
						  set(i,j,value(k,j));
						  set(k,j,t);
						}
					 for (j=0; j<rownum; j++)
						{
						  t = value(j,i);
						  set(j,i,value(j,k));
						  set(j,k,t);
						}
				  }
				for (i=k+1; i<rownum; i++)
				  {
					 t = value(i,k-1)/d;
					 set(i,k-1,0.0);
					 for (j=k; j<rownum; j++)
						  set(i,j,value(i,j)-t*value(k,j));
					 for (j=0; j<rownum; j++)
						  set(j,k,value(j,k)+t*value(j,i));
				  }
			 }
		}
}

void matrix::qreigen(matrix & u1, matrix & v1, size_t jt, DOUBLE eps)
 // 求一般实矩阵的所有特征根
// a和b均返回rownum行一列的列向量矩阵，返回所有特征根的实部和虚部
{
	matrix a(*this);
	a.hessenberg();	// 先算出赫申伯格矩阵
	u1 = matrix(rownum);
	v1 = matrix(rownum);
	buffer * uu = getnewbuffer(rownum);
	buffer * vv = getnewbuffer(rownum);
	buffer &u = *uu;
	buffer &v = *vv;
	 size_t m,it,i,j,k,l;
	 size_t iir,iic,jjr,jjc,kkr,kkc,llr,llc;
	 DOUBLE b,c,w,g,xy,p,q,r,x,s,e,f,z,y;
	 it=0; m=rownum;
	 while (m!=0)
		{ l=m-1;
		  while ( l>0 && (fabs(a.value(l,l-1))>eps*
			(fabs(a.value(l-1,l-1))+fabs(a.value(l,l))))) l--;
		  iir = m-1; iic = m-1;
		  jjr = m-1; jjc = m-2;
		  kkr = m-2; kkc = m-1;
		  llr = m-2; llc = m-2;
		  if (l==m-1)
			 { u[m-1]=a.value(m-1,m-1); v[m-1]=0.0;
				m--; it=0;
			 }
		  else if (l==m-2)
			 { b=-(a.value(iir,iic)+a.value(llr,llc));
				c=a.value(iir,iic)*a.value(llr,llc)-
					a.value(jjr,jjc)*a.value(kkr,kkc);
				w=b*b-4.0*c;
				y=sqrt(fabs(w));
				if (w>0.0)
				  { xy=1.0;
					 if (b<0.0) xy=-1.0;
					 u[m-1]=(-b-xy*y)/2.0;
					 u[m-2]=c/u[m-1];
					 v[m-1]=0.0; v[m-2]=0.0;
				  }
				else
				  { u[m-1]=-b/2.0; u[m-2]=u[m-1];
					 v[m-1]=y/2.0; v[m-2]=-v[m-1];
				  }
				m=m-2; it=0;
			 }
		  else
			 {
			 if (it>=jt) {
				delete uu;
				delete vv;
				throw TMESSAGE("fail to calculate eigenvalue");
			 }
				it++;
				for (j=l+2; j<m; j++)
					a.set(j,j-2,0.0);
				for (j=l+3; j<m; j++)
					a.set(j,j-3,0.0);
				for (k=l; k+1<m; k++)
				  { if (k!=l)
						{ p=a.value(k,k-1); q=a.value(k+1,k-1);
						  r=0.0;
						  if (k!=m-2) r=a.value(k+2,k-1);
						}
					 else
						{
						  x=a.value(iir,iic)+a.value(llr,llc);
						  y=a.value(llr,llc)*a.value(iir,iic)-
								a.value(kkr,kkc)*a.value(jjr,jjc);
						  iir = l; iic = l;
						  jjr = l; jjc = l+1;
						  kkr = l+1; kkc = l;
						  llr = l+1; llc = l+1;
						  p=a.value(iir,iic)*(a.value(iir,iic)-x)
								+a.value(jjr,jjc)*a.value(kkr,kkc)+y;
						  q=a.value(kkr,kkc)*(a.value(iir,iic)+a.value(llr,llc)-x);
						  r=a.value(kkr,kkc)*a.value(l+2,l+1);
						}
					 if ((fabs(p)+fabs(q)+fabs(r))!=0.0)
						{ xy=1.0;
						  if (p<0.0) xy=-1.0;
						  s=xy*sqrt(p*p+q*q+r*r);
						  if (k!=l) a.set(k,k-1,-s);
						  e=-q/s; f=-r/s; x=-p/s;
						  y=-x-f*r/(p+s);
						  g=e*r/(p+s);
						  z=-x-e*q/(p+s);
						  for (j=k; j<m; j++)
							 {
								iir = k; iic = j;
								jjr = k+1; jjc = j;
								p=x*a.value(iir,iic)+e*a.value(jjr,jjc);
								q=e*a.value(iir,iic)+y*a.value(jjr,jjc);
								r=f*a.value(iir,iic)+g*a.value(jjr,jjc);
								if (k!=m-2)
								  { kkr = k+2; kkc = j;
									 p=p+f*a.value(kkr,kkc);
									 q=q+g*a.value(kkr,kkc);
									 r=r+z*a.value(kkr,kkc);
									 a.set(kkr,kkc,r);
								  }
								a.set(jjr,jjc,q);
								a.set(iir,iic,p);
							 }
						  j=k+3;
						  if (j>=m-1) j=m-1;
						  for (i=l; i<=j; i++)
							 { iir = i; iic = k;
								jjr = i; jjc = k+1;
								p=x*a.value(iir,iic)+e*a.value(jjr,jjc);
								q=e*a.value(iir,iic)+y*a.value(jjr,jjc);
								r=f*a.value(iir,iic)+g*a.value(jjr,jjc);
								if (k!=m-2)
								  { kkr = i; kkc = k+2;
									 p=p+f*a.value(kkr,kkc);
									 q=q+g*a.value(kkr,kkc);
									 r=r+z*a.value(kkr,kkc);
									 a.set(kkr,kkc,r);
								  }
								a.set(jjr,jjc,q);
								a.set(iir,iic,p);
							 }
						}
				  }
			 }
		}
	for(i=0;i<rownum;i++) {
		u1.set(i,u[i]);
		v1.set(i,v[i]);
	}
	delete uu;
	delete vv;
}

DOUBLE gassrand(int rr)	// 返回一零均值单位方差的正态分布随机数
{
	static DOUBLE r=3.0;	// 种子
	if(rr) r = rr;
	int i,m;
	DOUBLE s,w,v,t;
	s=65536.0; w=2053.0; v=13849.0;
	t=0.0;
	for (i=1; i<=12; i++)
		{ r=r*w+v; m=(int)(r/s);
		  r-=m*s; t+=r/s;
		}
	t-=6.0;
	return(t);
}

gassvector::gassvector(matrix & r):	//r必须是正定对称阵，为正态随机向量的协方差
	a(r)
{
	if(r.rownum != r.colnum) throw TMESSAGE("must be a sqare matrix");
	if(!r.issym) throw TMESSAGE("must be a symetic matrix");
	matrix evalue;
	a = a.eigen(evalue);
	for(size_t i=0; i<a.colnum; i++) {
		DOUBLE e = sqrt(evalue(i));
		for(size_t j=0; j<a.rownum; j++)
			a.set(j,i,a.value(j,i)*e);
	}
}

matrix gassvector::operator()(matrix & r) // 返回给定协方差矩阵的正态随机向量
{
	a = r;
	matrix evalue;
	a = a.eigen(evalue);
	size_t i;
	for(i=0; i<a.colnum; i++) {
		DOUBLE e = sqrt(evalue(i));
		for(size_t j=0; j<a.rownum; j++)
			a.set(j,i,a.value(j,i)*e);
	}
	matrix rr(a.rownum);	// 产生列向量
	for(i=0; i<a.rownum; i++)
		rr.set(i,gassrand());
	return a*rr;
}

matrix gassvector::operator()()	// 返回已设定的协方差矩阵的正态随机向量
{
	matrix rr(a.rownum);
	for(size_t i=0; i<a.rownum; i++)
		rr.set(i,gassrand());
	return a*rr;
}

void gassvector::setdata(matrix & r) // 根据协方差矩阵设置增益矩阵
{
	if(!r.issym) throw TMESSAGE("r must be symetic!");
	a = r;
	matrix evalue;
	a = a.eigen(evalue);
	for(size_t i=0; i<a.colnum; i++) {
   	if(evalue(i)<0.0) throw TMESSAGE("var matrix not positive!");
		DOUBLE e = sqrt(evalue(i));
		for(size_t j=0; j<a.rownum; j++)
			a.set(j,i,a.value(j,i)*e);
	}