arithmetic.cpp

介绍一些常用的数值计算方法

#include "arithmetic.h"
#include "math.h"
#include "matrix.h"
#include "iostream.h"

void math_fft(double *pr,double *pi,double *fr,double *fi,
			  int n,int k,int l,int il)
{
	//用FFT计算离散傅立叶(Fourier)变换
	int it,m,is,i,j,nv,l0;
	double p,q,s,vr,vi,poddr,poddi;
	for(it=0;it<n;it++)
	{
		m=it;
		is=0;
		for(i=0;i<k;i++)
		{
			j=m/2;
			is=2*is+(m-2*j);//因为是整形数的运算，会去掉一些小数部分
			m=j;
		}
		*(fr+it)=*(pr+is);
		*(fi+it)=*(pi+is);
	}
	*pr=1.0;
	*pi=0.0;
	p=6.283185306/(1.0*n);
	*(pr+1)=cos(p);
	*(pi+1)=-sin(p);
	if(l!=0)
		*(pi+1)=-*(pi+1);
	for(i=2;i<n;i++)
	{
		p=*(pr+i-1)**(pr+1);
		q=*(pi+i-1)**(pi+1);
		s=(*(pr+i-1)+*(pi+i-1))*(*(pr+1)+*(pi+1));
		*(pr+i)=p-q;
		*(pi+i)=s-p-q;
	}
	for(it=0;it<n-1;it=it+2)  //
	{
		vr=*(fr+it);
		vi=*(fi+it);
		*(fr+it)=vr+*(fr+it+1);
		*(fi+it)=vi+*(fi+it+1);
		*(fr+it+1)=vr-*(fr+it+1);
		*(fi+it+1)=vi-*(fi+it+1);
	}
	m=n/2;
	nv=2;
	for(l0=k-2;l0>=0;l0--)//对半进行FFT算法
	{
		m=m/2;
		nv=2*nv;
		for(it=0;it<=(m-1)*nv;it=it+nv)
			for(j=0;j<=(nv/2)-1;j++)
			{
				p=*(pr+m*j)**(fr+it+j+nv/2);
				q=*(pi+m*j)**(fi+it+j+nv/2);
				s=*(pr+m*j)+*(pi+m*j);
				s=s*(*(fr+it+j+nv/2)+*(fi+it+j+nv/2));
				poddr=p-q;
				poddi=s-p-q;
				*(fr+it+j+nv/2)=*(fr+it+j)-poddr;
				*(fi+it+j+nv/2)=*(fi+it+j)-poddi;
				*(fr+it+j)=*(fr+it+j)+poddr;
				*(fi+it+j)=*(fi+it+j)+poddi;
			}
	}
	if(l!=0)
		for(i=0;i<n;i++)
		{
			*(fr+i)=*(fr+i)/(1.0*n);
			*(fi+i)=*(fi+i)/(1.0*n);
		}
	if(il!=0)
		for(i=0;i<n;i++)
		{
			*(pr+i)=sqrt(*(fr+i)**(fr+i)+*(fi+i)**(fi+i));
			if(fabs(*(fr+i))<0.000001*fabs(*(fi+i)))
			{
				if(*(fi+i)**(fr+i)>0)
					*(pi+i)=90.0;
				else 
					*(pi+i)=-90.0;
			}
			else
				*(pi+i)=atan(*(fi+i)/(*(fr+i)))*360.0/6.283185306;
		}
		return ;
}

int math_kalman(double *f,double *q,double *r,double *h,double *y,double *x,
				double *p,double *g,int n,int m,int k)
{
	//离散随机线性系统的kalman滤波
	int i,j,kk,ii,l,jj;
	double e[100],a[100],b[100];//这样定义，处理的阶数不能超过10阶
	l=m;
	if(l<n)
		l=n;
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
		{
			ii=i*l+j;
			a[ii]=0;
			for(kk=0;kk<n;kk++)
				a[ii]=a[ii]+*(p+i*n+kk)**(f+j*n+kk);//a=p*H
		}
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
		{
			ii=i*n+j;
			*(p+ii)=*(q+ii);
			for(kk=0;kk<n;kk++)
				*(p+ii)=*(p+ii)+*(f+i*n+kk)**(a+kk*l+j);//p=
		}
	for(ii=2;ii<=k;ii++)
	{
		for(i=0;i<n;i++)
			for(j=0;j<m;j++)
			{
				jj=i*l+j;
				a[jj]=0;
				for(kk=0;kk<n;kk++)
					a[jj]=a[jj]+*(p+i*n+kk)**(h+j*n+kk);//a=P*H(trans)
			}
		for(i=0;i<m;i++)
			for(j=0;j<m;j++)
			{
				jj=i*m+j;
				e[jj]=*(r+jj);//e=R,R模型噪声的协方差矩阵
				for(kk=0;kk<n;kk++)
					e[jj]=e[jj]+*(h+i*n+kk)*a[kk*l+j];//e=H*P*H(trans)+R
			}
	//	void matrix_qiuni(double *,int n);
		i=matrix_qiuni((double *)e,m);
		if(i<0)
		{
			cout<<"fail\n!";
			return -1;
		}
		for(i=0;i<n;i++)
			for(j=0;j<m;j++)
			{
				jj=i*m+j;
				*(g+jj)=0.0;
				for(kk=0;kk<m;kk++)
					*(g+jj)=*(g+jj)+a[i*l+kk]*e[j*m+kk];//G=P*H*inv[H*P*H(trans)+R]
			}
			for(i=0;i<n;i++)
			{
				jj=(ii-1)*n+i;
				*(x+jj)=0.0;
				for(j=0;j<n;j++)
					*(x+jj)=*(x+jj)+*(f+i*n+j)**(x+(ii-2)*n+j);
				//X=F*X
				//没有破坏原来的数据，还是马上重新利用？
			}
			for(i=0;i<m;i++)
			{
				jj=i*l;
				b[jj]=*(y+(ii-1)*m+i);//B=Y
				for(j=0;j<n;j++)
					b[jj]=b[jj]-*(h+i*n+j)**(x+(ii-1)*n+j);
				//B=Y-H*F*X=Y-H*X
			}
			for(i=0;i<n;i++)
			{
				jj=(ii-1)*n+i;
				for(j=0;j<m;j++)
					*(x+jj)=*(x+jj)+*(g+i*m+j)*b[j*l];
				//X=G*B=G*[Y-H*F*X]
			}
			if(ii<k)
			{
				for(i=0;i<n;i++)
					for(j=0;j<n;j++)
					{
						jj=i*l+j;
						a[jj]=0;
						for(kk=0;kk<m;kk++)
							a[jj]=a[jj]-*(g+i*m+kk)**(h+kk*n+j);//A=-G*H
						if(i==j)
							a[jj]=1.0+a[jj];//A=I-G*H
					}
				for(i=0;i<n;i++)
					for(j=0;j<n;j++)
					{
						jj=i*l+j;
						b[jj]=0.0;
						for(kk=0;kk<n;kk++)
							b[jj]=b[jj]+*(a+i*l+kk)**(p+kk*n+j);//B=(I-G*H)*P
					}
				for(i=0;i<n;i++)
					for(j=0;j<n;j++)
					{
						jj=i*l+j;
						a[jj]=0.0;
						for(kk=0;kk<n;kk++)
							a[jj]=a[jj]+b[i*l+kk]**(f+j*n+kk);//A=B*F
					}
				for(i=0;i<n;i++)
					for(j=0;j<n;j++)
					{
						jj=i*n+j;
						*(p+jj)=*(q+jj);//P=Q
						for(kk=0;kk<n;kk++)
							*(p+jj)=*(p+jj)+*(f+i*n+kk)*a[j*l+kk];//P=F*B=F*B*F+Q
					}
			}
	}
	return 1;
}

void albeigan(double *x,double *y,double t,double a,double b,double c,int n)
{
	//alpha-beida-ganma滤波
	int i;
	double s1,ss,v1,vv,a1,aa;
	aa=0;
	vv=0;
	ss=0;
	for(i=0;i<n;i++)
	{
		s1=ss+t*vv+t*t*aa/2;
		v1=vv+t*aa;
		a1=aa;
		ss=s1+a*(*(x+i)-s1);
		*(y+i)=ss;
		vv=v1+b*(*(x+i)-s1);//除以t?,没有行吗？
		aa=a1+2.0*c*(*(x+i)-s1)/(t*t);
	}
	return ;
}