vfunc.cpp

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

vfunc& vfunc::operator*=(matrix& (*f)(matrix&)) // 自身乘一个函数指针
{
	vfunc fn(f);	// 将函数指针包装为函数类
	operator*=(fn);	// 实施乘操作
	return (*this);
}

vfunc vfunc::operator*(vfunc& fn)	// 相乘产生新函数
{
	valgo * a = new valgojoin(alg, fn.alg, cmul);
	vfunc f(a);
	return f;
}

vfunc vfunc::operator*(DOUBLE a)	// 与常数相乘产生新函数
{
	vfunc f(*this);
	f *= a;
	return f;
}

vfunc vfunc::operator*(matrix& (*f)(matrix&)) // 乘一个函数指针产生新函数
{
	vfunc ff(*this);
	ff *= f;
	return ff;
}

vfunc& vfunc::operator/=(vfunc& fn) // 自身除以一个函数
{
	valgo * a = new valgojoin(alg, fn.alg, cdiv);
	alg->refnum--;	// 因为联合算法对两个算法都加了引用数，因此再减回来
	alg = a;	// 将新指针赋给alg
	return (*this);
}

vfunc& vfunc::operator/=(matrix& (*f)(matrix&)) // 自身除以一个函数指针
{
	vfunc fn(f);	// 将函数指针包装为函数类
	operator/=(fn);	// 实施除法操作
	return (*this);
}

vfunc vfunc::operator/(vfunc& fn)	// 相除产生新函数
{
	valgo * a = new valgojoin(alg, fn.alg, cdiv);
	vfunc f(a);
	return f;
}

vfunc vfunc::operator/(DOUBLE a)	// 与常数相除产生新函数
{
	vfunc f(*this);
	f /= a;
	return f;
}

vfunc operator/(DOUBLE a, vfunc& f) // 常数除以函数
{
	vfunc ff(a);
	return ff/f;
}

vfunc vfunc::operator/(matrix& (*f)(matrix&)) // 除以一个函数指针产生新函数
{
	vfunc ff(*this);
	ff /= f;
	return ff;
}

vfunc operator/(matrix& (*f)(matrix&),vfunc& fn) // 函数指针除以函数
{
	vfunc ff(f);
	ff /= fn;
	return ff;
}

void vfunc::setxfactor(DOUBLE a)	// 设置x因子为a
{
	alg = alg->setxfactor(a);
}

void vfunc::xroom(DOUBLE a)	  // x方向扩大a倍
{
	alg = alg->xroom(a);
}

vfunc vfunc::operator()(vfunc & fn)	// 复合函数，产生新的函数
{
	valgo * a = new valgojoin(alg, fn.alg, ccom);
	vfunc f(a);
	return f;
}

void vfunc::setxshift(DOUBLE a)	// 设置函数沿x轴平移a
{
	alg = alg->setxshift(a);
}

void vfunc::shiftxas(DOUBLE a) // 函数沿x轴右移a
{
	alg = alg->xshiftas(a);
}

//////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////

linemodel::linemodel(matrix& va, matrix & vh, matrix & q,
	 matrix & r, matrix & vx) :a(va),h(vh),w(q),v(r),x(vx),  //初始化
		y(vh.rownum,1)
			// (构造函数)，va初始状态转移矩阵，vh初始观测矩阵，q当前模型噪声协
			// 方差阵，r当前观测噪声协方差阵，vx初始状态变量
{
	if(va.rownum != vx.rownum ||
		va.rownum != va.colnum ||
		vh.colnum != va.rownum ||
		q.rownum != va.rownum ||
		r.rownum != vh.rownum) throw TMESSAGE("Data not meet need!");
	y = h*x+v();
//	y = 0.0;
};

void linemodel::setdata(matrix& va, matrix & vh, matrix & q, matrix & r)
{
	if(va.rownum != va.colnum ||
		va.rownum != x.rownum ||
		vh.colnum != va.rownum ||
		q.rownum != va.rownum ||
		r.rownum != vh.rownum) throw TMESSAGE("Data not meet need!");
	a = va;
	h = vh;
	w.setdata(q);
	v.setdata(r);
}

void linemodel::seta(matrix& va)
{
	if(va.rownum != va.colnum ||
		va.rownum != x.rownum) throw TMESSAGE("a not meet need!");
	a = va;
}

void linemodel::seth(matrix& vh)
{
	if(vh.rownum != y.rownum ||
		vh.colnum != x.rownum ) throw TMESSAGE("h not meet nead!");
	h = vh;
}

void linemodel::setq(matrix& q)
{
	if(q.rownum != x.rownum ||
		!q.issym) throw TMESSAGE("q not meet nead!");
	w.setdata(q);
}

void linemodel::setr(matrix& r)
{
	if(r.rownum != y.rownum ||
		!r.issym) throw TMESSAGE("r not meet nead!");
	v.setdata(r);
}

matrix & linemodel::next(int n)  //next()	// 计算下一级x值并返回新的x对应的y值
{
//	x = a*x+w();		// 产生新的状态变量真实值，w()产生随机变量
//	return (y=h*x+v());	// 产生并返回新的观测值，v()产生随机变量

	if(1==n)
	{
		x=a*x;
		double g1[2][1];
		g1[0][0]=(x(4)*x(2)-x(1)*x(5))/(x(1)*x(1)+x(4)*x(4));
		g1[1][0]=sqrt(x(1)*x(1)+x(4)*x(4));
		matrix t1(g1,2,1);
		return (y=t1+v());
	}
	if(2==n)
	{
		x=a*x;
		double g2[4][1];
		g2[0][0]=x(0);g2[1][0]=x(3);
		g2[2][0]=(x(4)*x(2)-x(1)*x(5))/(x(1)*x(1)+x(4)*x(4));
		g2[3][0]=sqrt(x(1)*x(1)+x(4)*x(4));
		matrix t2(g2,4,1);
		return (y=t2+v());
	}
	if(0==n)
	{
		x=a*x;  // 产生新的状态变量真实值
		return (y=h*x+v());  // 产生并返回新的观测值
	}
	else
	{
		x=a*x;  // 产生新的状态变量真实值
		return (y=h*x+v());  // 产生并返回新的观测值
	}
}

kalman::kalman(matrix &va,matrix& vq,matrix& vr,matrix& vh,matrix& vx,
	           matrix& vp):
		// 构造函数，va为状态转移矩阵，vh观测矩阵，vq模型噪声协方差阵，
		// vr当前观测噪声协方差阵，vx初始状态变量估值，vp初始估值协方差阵
	a(va),q(vq),r(vr),h(vh),x(vx),p(vp),y(vh.rownum,1)
{
	if( va.rownum != va.colnum ||
		 !vq.issym || !vr.issym || !vp.issym ||
		 vq.rownum != va.rownum || vr.rownum != vh.rownum ||
		 vx.rownum != va.rownum || vp.rownum != vx.rownum ||
		 vh.colnum != vx.rownum ) 
		 throw TMESSAGE("data not meet need!");
	if( !vq.ispositive() || !vr.ispositive() )
		 throw TMESSAGE("var matrix not positive!");
	y = 0.0;
}

void kalman::setdata(matrix &va,matrix& vq,matrix& vr,matrix& vh)
		// 为时变系统随时设置系统参数，va为状态转移矩阵，vh观测矩阵，vq模型噪
		// 声协方差阵，vr当前观测噪声协方差阵
{
	if(va.rownum != va.colnum || va.rownum != x.rownum ||
		va.rownum != vq.rownum || !vq.issym ||
		vr.rownum != y.rownum || vh.rownum != y.rownum ||
		vh.colnum != x.rownum ) throw TMESSAGE("data not meet need!");
	if( !vq.ispositive() || !vr.ispositive() )
	 throw TMESSAGE("var matrix not positive!");
	a = va;
	q = vq;
	h = vh;
	r = vr;
}

void kalman::seta(matrix& va)
{
	if(va.rownum != va.colnum || va.rownum != x.rownum)
		throw TMESSAGE("data not meet need!");
	a = va;
}

void kalman::seth(matrix& vh)
{
	if(vh.rownum != y.rownum || vh.colnum != x.rownum)
		throw TMESSAGE("data not meet need!");
	h = vh;
}

void kalman::setq(matrix& vq)
{
	if(vq.rownum != x.rownum || !vq.issym)
		throw TMESSAGE("data not meet need!");
	if(!vq.ispositive())
		throw TMESSAGE("q is not positive!");
	q = vq;
}

void kalman::setr(matrix& vr)
{
	if(vr.rownum != y.rownum || !vr.issym)
		throw TMESSAGE("data not meet need!");
	if(!vr.ispositive())
		throw TMESSAGE("r is not positive!");
	r = vr;
}

/*matrix& kalman::next()	// 预测下一个状态变量的值
{
//	x = a*x;	// 预测
	return x;
}*/

matrix& kalman::next(matrix& y,int n)  // 根据下一个观测值获得新的状态变量的估值
{                                      //next(matrix& y)
//	double g[2][1];
//	g[0][0]=x(0);g[1][0]=x(3);
//	g[0][0]=(x(4)*x(2)-x(1)*x(5))/(x(1)*x(1)+x(4)*x(4));
//	g[1][0]=sqrt(x(1)*x(1)+x(4)*x(4));
//	matrix t(g,2,1);
	
	matrix t;
	if(1==n)
	{
		double g1[2][1];
		g1[0][0]=(x(4)*x(2)-x(1)*x(5))/(x(1)*x(1)+x(4)*x(4));
		g1[1][0]=sqrt(x(1)*x(1)+x(4)*x(4));
		matrix t1(g1,2,1);
		t=t1;
	}
	if(2==n)
	{
		double g2[4][1];
		g2[0][0]=x(0);g2[1][0]=x(3);
		g2[2][0]=(x(4)*x(2)-x(1)*x(5))/(x(1)*x(1)+x(4)*x(4));
		g2[3][0]=sqrt(x(1)*x(1)+x(4)*x(4));
		matrix t2(g2,4,1);
		t=t2;
	}
	if(0==n)
		t=h*x;
	if(3==n)
		t=h*x;
	
	x = a*x;		// 预测(闭环滤波系统，预测为0)
	p = a*p*a.t()+q;	// 预测误差协方差
	matrix k=p*h.t()*((h*p*h.t()+r).inv()); // 新息的增益
//	x += k*(y-h*x);	// 由新息校正x，得现时估计
	x += k*(y-t);	// 由新息校正x，得现时估计
	p = (unit(p.rownum)-k*h)*p;// 获得最后的误差协方差阵，unit(n)是n阶单位矩阵
//	p = (unit(p.rownum)-k*h)*p*(unit(p.rownum)-k*h).t()+k*r*k.t();
	return x;	// 返回x的估计值
}


regress::regress(matrix& x, matrix& y)	// x为mXn维矩阵，y为n个观测值
{
	if(x.colnum != y.rownum || x.colnum < x.rownum)
		throw TMESSAGE("dimension error");
	matrix c(x.rownum+1,x.colnum);
	size_t i,j;
	for(i=0; i<x.rownum; i++)
	for(j=0; j<x.colnum; j++)
		c.set(i,j,x(i,j));
	for(j=0; j<c.colnum; j++)
		c.set(x.rownum,j,1.0);
	matrix y1(c*y);
	c *= c.t();
	a = c.cholesky(y1);
	v = matrix(x.rownum);
	// 计算偏差平方和
	q = 0;
	DOUBLE yy;
	matrix dd(y.rownum);
	for(i=0; i<y.rownum; i++) {
		yy = y(i);
		for(j=0; j<x.rownum; j++)
			yy -= a(j)*x(j,i);
		yy -= a(a.rownum-1);
		dd.set(i,yy);
		q += yy*yy;
	}
	s = sqrt(q/(y.rownum));
	yy = 0.0;
	for(i=0; i<y.rownum; i++)
		yy += y(i);
	DOUBLE ye;
	ye = yy/(y.rownum);
	r = 0.0;
	for(i=0; i<y.rownum; i++)
		r += (y(i)-ye)*(y(i)-ye);
	r = sqrt(1.0-q/r);
	for(j=0;j<v.rownum;j++) {
		yy = 0;
		for(i=0;i<y.rownum;i++)
			yy += (dd(i)+a(j)*x(j,i))*(dd(i)+a(j)*x(j,i));
		v.set(j,sqrt(1.0-q/yy));
	}
	u = 0.0;
	for(i=0; i<y.rownum; i++) {
		yy = ye;
		for(j=0; j<x.rownum; j++)
			yy -= a(j)*x(j,i);
		yy -= a(x.rownum);
		u += yy*yy;
	}
}

DOUBLE regress::operator()(matrix& x) // 回归后的线性函数，x是自变量
{
	if(x.rownum != a.rownum-1) throw TMESSAGE("wrong dimension!");
	DOUBLE y = a(a.rownum-1);
	for(size_t i=0; i<x.rownum; i++)
		y += x(i)*a(i);
	return y;
}