matrix.cpp

基于VC环境下的组合导航卡尔曼滤波仿真器设计

 /* 本矩阵其实是常数阵，而矩阵m必须是方阵
	运算过程其实是对本矩阵和m同时作行初等变换，
	运算结果m的对角线相乘将是行列式，而本矩阵则变换成
	自己的原矩阵被m的逆阵左乘，m的秩被返回，如果秩等于阶数
	则本矩阵中的内容已经是唯一解
 */
{
	if(rownum != m.rownum || m.rownum != m.colnum) // 本矩阵行数必须与m相等
						// 且m必须是方阵
		throw TMESSAGE("can not divide!");
	lbuffer * bb = getnewlbuffer(rownum); // 产生一维数为行数的长整数缓存区
	lbuffer & b = (*bb); // 用引用的办法使下面的程序容易懂
	size_t is;
	DOUBLE a;
	size_t i,j,rank=0;
	for(size_t k=0; k<rownum; k++) { // 从第0行到第k行进行主高斯消元
		if(m.maxabs(is, i, k)==0) // 求m中第k级主元，主元所在的行，列存在is,i中
			break; // 如果主元为零，则m不可逆，运算失败
		rank = k+1; // rank存放当前的阶数
		b.retrieve(k) = i;  // 将长整数缓存区的第k个值设为i
		if(i != k)
			m.swapc(k, i); // 交换m中i,k两列
		if(is != k) {
			m.swapr(k, is, k); // 交换m中i,k两行,从k列以后交换
			swapr(k, is); // 交换本矩阵中i,k两行
		}
		a = m.value(k,k);  // 取出主元元素
		for (j=k+1;j<rownum;j++) // 本意是将m的第k行除以主元
				// 但只需把第k行的第k+1列以上除以主元即可
				// 这样还保留了主元作行列式运算用
			m.set(k,j,m.value(k,j)/a);
		for (j=0;j<colnum;j++) // 将本矩阵的第k行除以主元
			set(k,j,value(k,j)/a);
		// 上面两步相当于将m和本矩阵构成的增广矩阵第k行除以主元
		// 下面对增广矩阵作行基本初等变换使第k行的其余列均为零
		// 但0值无必要计算，因此从第k+1列开始计算
		for(j=k+1;j<rownum;j++) // j代表列，本矩阵的行数就是m的列数
		for(i=0;i<rownum;i++) //i代表行，依次对各行计算，k行除外
			if(i!=k)
				m.set(i,j,m.value(i,j)-m.value(i,k)*m.value(k,j));
		// 对本矩阵亦作同样的计算
		for(j=0;j<colnum;j++)
		for(i=0;i<rownum;i++)
			if(i!=k)
				set(i,j,value(i,j)-m.value(i,k)*value(k,j));
	} // 主高斯消元循环k结束
	if(fn == 1) {
		for(j=0; j<rank; j++)
		for(i=0; i<rownum; i++)
			if(i==j) m.set(i,i,1.0);
			else
				m.set(i,j,0.0);
		for(k = rank; k>0; k--)
			m.swapc(k-1,(size_t)b[k-1]);
	}
	for(k = rank; k>0; k--) // 将本矩阵中的各行按b中内容进行调节
		if(b[k-1] != k-1)
			swapr(k-1,(size_t)b[k-1]); // 行交换
	delete bb; // 释放长整数缓存
	return rank; // 返回mm的秩
}

matrix& matrix::operator/=(matrix m) // 利用重载的除法符号/=来解方程
	// 本矩阵设为d,则方程为mx=d,考虑解写成x=d/m的形式，
	// 而方程的解也存放在d中，则实际编程时写d/=m
{
	if(zgsxy(m)!=rownum) // 如秩不等于m的阶数，则方程无解
		throw TMESSAGE("can not divide!");
	return *this;
}

matrix matrix::operator/(matrix m) // 左乘m的逆矩阵产生新矩阵
{
	m.inv();	// m的逆矩阵
	return (*this)*m;
}

matrix& matrix::inv()		// 用全选主元高斯-约当法求逆矩阵
{
	if(rownum != colnum || rownum == 0)
		throw TMESSAGE("Can not calculate inverse");
	size_t i,j,k;
	 DOUBLE d,p;
	lbuffer * isp = getnewlbuffer(rownum); // 产生一维数为行数的长整数缓存区
	lbuffer * jsp = getnewlbuffer(rownum); // 产生一维数为行数的长整数缓存区
	lbuffer& is = *isp;	// 使用引用使程序看起来方便
	lbuffer& js = *jsp;
	for(k=0; k<rownum; k++)
		{
			d = maxabs(i, j, k); // 全主元的位置和值
			is[k] = i;
			js[k] = j;
			if(d==0.0) {
				delete isp;
				delete jsp;
				throw TMESSAGE("can not inverse");
			}
		  if (is[k] != k) swapr(k,(size_t)is[k]);
		  if (js[k] != k) swapc(k,(size_t)js[k]);
			p = 1.0/value(k,k);
			set(k,k,p);
		  for (j=0; j<rownum; j++)
			 if (j!=k) set(k,j,value(k,j)*p);
		  for (i=0; i<rownum; i++)
			 if (i!=k)
				for (j=0; j<rownum; j++)
				  if (j!=k) set(i,j,value(i,j)-value(i,k)*value(k,j));
		  for (i=0; i<rownum; i++)
			 if (i!=k) set(i,k,-value(i,k)*p);
		} // end for k
	 for (k=rownum; k>0; k--)
		{ if (js[k-1]!=k-1) swapr((size_t)js[k-1], k-1);
		  if (is[k-1]!=k-1) swapc((size_t)is[k-1], k-1);
		}
  delete isp;
  delete jsp;
	checksym();	// 检查是否为对称阵
  return (*this);
}

matrix matrix::operator~()	// 求逆矩阵，但产生新矩阵
{
	matrix m(*this);
	m.inv();
	return m;
}

matrix operator/(DOUBLE a, matrix& m) // 求逆矩阵再乘常数
{
	matrix mm(m);
	mm.inv();
	if(a != 1.0) mm*=a;
	return mm;
}

matrix& matrix::operator/=(DOUBLE a) // 所有元素乘a的倒数，自身改变
{
	return operator*=(1/a);
}

matrix matrix::operator/(DOUBLE a) // 所有元素乘a的倒数，产生新的矩阵
{
	matrix m(*this);
	m/=a;
	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++)