分类： C/C++
2012-04-29 05:16:10
数值计算程序大放送-数学变换与滤波
//////////////////////////////////////////////////////////////
//傅里叶级数逼近
//f-长度为2n+1的数组，存放[0,2Pi]上2n+1个等距点处的函数值
//n-整型变量
//a-长度为n+1的数组，返回时存放傅里叶级数系数ak
//b-长度为n+1的数组，返回时存放傅里叶级数系数bk
void kfour(double f[],int n,double a[],double b[]);
//////////////////////////////////////////////////////////////
//快速离散傅里叶变换（FFT）
//pr-长度为n的数组，当l=0时，存放n个采样输入的实部，返回时存放离散傅里叶变换的模；
// 当l=1时，存放傅里叶变换的n个实部，返回时存放逆傅里叶变换的模；
//pi-长度为n的数组，当l=0时，存放n个采样输入的虚部，返回时存放离散傅里叶变换的幅角（单位为度）；
// 当l=1时，存放傅里叶变换的n个虚部，返回时存放逆傅里叶变换的幅角（单位为度）；
//n-整型变量，输入的点数
//k-整型变量，满足n=2^k
//fr-长度为n的数组，当l=0时，返回时存放离散傅里叶变换的实部；
// 当l=1时，返回时存放逆傅里叶变换的实部
//fi-长度为n的数组，当l=0时，返回时存放离散傅里叶变换的虚部；
// 当l=1时，返回时存放逆傅里叶变换的虚部
//l-整型变量，l=1时，计算傅里叶变换，当l=0时，计算逆傅里叶变换
//il-整型变量，il=0时，表示不要求计算傅里叶变换或逆变换的模与幅角
// il=1时，表示要求计算傅里叶变换或逆变换的模与幅角
void kkfft(double pr[],double pi[],int n,int k,double fr[],double fi[],int l,int il);
//////////////////////////////////////////////////////////////
//快速沃什（Walsh）变换
//p-长度为n的数组，存放n=2^k个给定的输入序列
//n-整型变量，输入的点数
//k-整型变量，满足n=2^k
//x-长度为n的数组，返回时存放沃什（Walsh）变换序列；
void kkfwt(double p[],double x[],int n,int k);
//////////////////////////////////////////////////////////////
//等距节点五点三次平滑
//n-整型变量，输入的点数，要求n>=5
//y-长度为n的数组，存放n个等距观测点上的观测数据
//yy-长度为n的数组，返回时存放平滑结果
void kkspt(int n,double y[],double yy[]);
//////////////////////////////////////////////////////////////
//离散随机线性系统的卡尔曼（kalman）滤波
//n-整型变量，动态系统的维数
//m-整型变量，观测系统的维数
//k-观测序列的长度
//f-n*n数组，系统状态转移矩阵
//q-n*n数组，模型噪声Wk的协方差矩阵
//r-m*m数组，观测噪声Vk的协方差矩阵
//h-m*n数组，观测矩阵
//yy-k*m数组，观测向量序列
//x-k*n数组，x[0,j]存放给定的初值，其余各行返回状态向量估计序列
//p-n*n数组，存放初值P0,返回时存放最后时刻的估计误差协方差矩阵
//g-n*m数组，返回最后时刻的稳定增益矩阵
//调用函数 brinv(double a[],int n);
int klman(int n,int m,int k,double f[],double q[],double r[],double h[],double y[],double x[],double p[],double g[]);
//////////////////////////////////////////////////////////////
//快速离散傅里叶变换（FFT）
//pr-长度为n的数组，当l=0时，存放n个采样输入的实部，返回时存放离散傅里叶变换的模；
// 当l=1时，存放傅里叶变换的n个实部，返回时存放逆傅里叶变换的模；
//pi-长度为n的数组，当l=0时，存放n个采样输入的虚部，返回时存放离散傅里叶变换的幅角（单位为度）；
// 当l=1时，存放傅里叶变换的n个虚部，返回时存放逆傅里叶变换的幅角（单位为度）；
//n-整型变量，输入的点数
//k-整型变量，满足n=2^k
//fr-长度为n的数组，当l=0时，返回时存放离散傅里叶变换的实部；
// 当l=1时，返回时存放逆傅里叶变换的实部
//fi-长度为n的数组，当l=0时，返回时存放离散傅里叶变换的虚部；
// 当l=1时，返回时存放逆傅里叶变换的虚部
//l-整型变量，l=1时，计算傅里叶变换，当l=0时，计算逆傅里叶变换
//il-整型变量，il=0时，表示不要求计算傅里叶变换或逆变换的模与幅角
// il=1时，表示要求计算傅里叶变换或逆变换的模与幅角
void kkfft(double pr[],double pi[],int n,int k,double fr[],double fi[],int l,int il)
{
int it,m,is,i,j,nv,l0;
double p,q,s,vr,vi,poddr,poddi;
for (it=0; it<=n-1; it++)
{
m=it; is=0;
for (i=0; i<=k-1; i++)
{
j=m/2; is=2*is+(m-2*j);
m=j;
}
fr[it]=pr[is];
fi[it]=pi[is];
}
pr[0]=1.0;
pi[0]=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-1; 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-2; 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--)
{
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-1; i++)
{
fr[i]=fr[i]/(1.0*n);
fi[i]=fi[i]/(1.0*n);
}
}
if (il!=0)
{
for (i=0; i<=n-1; 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;
}
//快速沃什（Walsh）变换
//p-长度为n的数组，存放n=2^k个给定的输入序列
//n-整型变量，输入的点数
//k-整型变量，满足n=2^k
//x-长度为n的数组，返回时存放沃什（Walsh）变换序列；
void kkfwt(double p[],double x[],int n,int k)
{
int m,l,it,ii,i,j,is;
double q;
m=1;
l=n;
it=2;
x[0]=1;
ii=n/2;
x[ii]=2;
for (i=1; i<=k-1; i++)
{
m=m+m;
l=l/2;
it=it+it;
for (j=0; j<=m-1; j++)
{
x[j*l+l/2]=it+1-x[j*l];
}
}
for (i=0; i<=n-1; i++)
{
ii=x[i]-1;
x[i]=p[ii];
}
l=1;
for (i=1; i<=k; i++)
{
m=n/(2*l)-1;
for (j=0; j<=m; j++)
{
it=2*l*j;
for (is=0; is<=l-1; is++)
{
q=x[it+is]+x[it+is+l];
x[it+is+l]=x[it+is]-x[it+is+l];
x[it+is]=q;
}
}
l=2*l;
}
return;
}
//等距节点五点三次平滑
//n-整型变量，输入的点数，要求n>=5
//y-长度为n的数组，存放n个等距观测点上的观测数据
//yy-长度为n的数组，返回时存放平滑结果
void kkspt(int n,double y[],double yy[])
{
int i;
if (n<5)
{
for (i=0; i<=n-1; i++)
{
yy[i]=y[i];
}
}
else
{
yy[0]=69.0*y[0]+4.0*y[1]-6.0*y[2]+4.0*y[3]-y[4];
yy[0]=yy[0]/70.0;
yy[1]=2.0*y[0]+27.0*y[1]+12.0*y[2]-8.0*y[3];
yy[1]=(yy[1]+2.0*y[4])/35.0;
for (i=2; i<=n-3; i++)
{
yy[i]=-3.0*y[i-2]+12.0*y[i-1]+17.0*y[i];
yy[i]=(yy[i]+12.0*y[i+1]-3.0*y[i+2])/35.0;
}
yy[n-2]=2.0*y[n-5]-8.0*y[n-4]+12.0*y[n-3];
yy[n-2]=(yy[n-2]+27.0*y[n-2]+2.0*y[n-1])/35.0;
yy[n-1]=-y[n-5]+4.0*y[n-4]-6.0*y[n-3];
yy[n-1]=(yy[n-1]+4.0*y[n-2]+69.0*y[n-1])/70.0;
}
return;
}
/离散随机线性系统的卡尔曼（kalman）滤波
//n-整型变量，动态系统的维数
//m-整型变量，观测系统的维数
//k-观测序列的长度
//f-n*n数组，系统状态转移矩阵
//q-n*n数组，模型噪声Wk的协方差矩阵
//r-m*m数组，观测噪声Vk的协方差矩阵
//h-m*n数组，观测矩阵
//yy-k*m数组，观测向量序列
//x-k*n数组，x[0,j]存放给定的初值，其余各行返回状态向量估计序列
//p-n*n数组，存放初值P0,返回时存放最后时刻的估计误差协方差矩阵
//g-n*m数组，返回最后时刻的稳定增益矩阵
int brinv(double a[],int n);
int klman(int n,int m,int k,double f[],double q[],double r[],double h[],double y[],double x[],double p[],double g[])
{
int i,j,kk,ii,l,jj,js;
double *e,*a,*b;
e=(double*)malloc(m*m*sizeof(double));
l=m;
if (l
l=n;
}
a=(double*)malloc(l*l*sizeof(double));
b=(double*)malloc(l*l*sizeof(double));
for (i=0; i<=n-1; i++)
{
for (j=0; j<=n-1; j++)
{
ii=i*l+j;
a[ii]=0.0;
for (kk=0; kk<=n-1; kk++)
{
a[ii]=a[ii]+p[i*n+kk]*f[j*n+kk];
}
}
}
for (i=0; i<=n-1; i++)
{
for (j=0; j<=n-1; j++)
{
ii=i*n+j;
p[ii]=q[ii];
for (kk=0; kk<=n-1; kk++)
{
p[ii]=p[ii]+f[i*n+kk]*a[kk*l+j];
}
}
}
for (ii=2; ii<=k; ii++)
{
for (i=0; i<=n-1; i++)
{
for (j=0; j<=m-1; j++)
{
jj=i*l+j;
a[jj]=0.0;
for (kk=0; kk<=n-1; kk++)
{
a[jj]=a[jj]+p[i*n+kk]*h[j*n+kk];
}
}
}
for (i=0; i<=m-1; i++)
{
for (j=0; j<=m-1; j++)
{
jj=i*m+j;
e[jj]=r[jj];
for (kk=0; kk<=n-1; kk++)
{
e[jj]=e[jj]+h[i*n+kk]*a[kk*l+j];
}
}
}
js=brinv(e,m);
if (js==0)
{
free(e);
free(a);
free(b);
return(js);
}
for (i=0; i<=n-1; i++)
{
for (j=0; j<=m-1; j++)
{
jj=i*m+j;
g[jj]=0.0;
for (kk=0; kk<=m-1; kk++)
{
g[jj]=g[jj]+a[i*l+kk]*e[j*m+kk];
}
}
}
for (i=0; i<=n-1; i++)
{
jj=(ii-1)*n+i;
x[jj]=0.0;
for (j=0; j<=n-1; j++)
{
x[jj]=x[jj]+f[i*n+j]*x[(ii-2)*n+j];
}
}
for (i=0; i<=m-1; i++)
{
jj=i*l;
b[jj]=y[(ii-1)*m+i];
for (j=0; j<=n-1; j++)
{
b[jj]=b[jj]-h[i*n+j]*x[(ii-1)*n+j];
}
}
for (i=0; i<=n-1; i++)
{
jj=(ii-1)*n+i;
for (j=0; j<=m-1; j++)
{
x[jj]=x[jj]+g[i*m+j]*b[j*l];
}
}
if (ii
for (i=0; i<=n-1; i++)
{
for (j=0; j<=n-1; j++)
{
jj=i*l+j;
a[jj]=0.0;
for (kk=0; kk<=m-1; kk++)
{
a[jj]=a[jj]-g[i*m+kk]*h[kk*n+j];
}
if (i==j)
{
a[jj]=1.0+a[jj];
}
}
}
for (i=0; i<=n-1; i++)
{
for (j=0; j<=n-1; j++)
{
jj=i*l+j;
b[jj]=0.0;
for (kk=0; kk<=n-1; kk++)
{
b[jj]=b[jj]+a[i*l+kk]*p[kk*n+j];
}
}
}
for (i=0; i<=n-1; i++)
{
for (j=0; j<=n-1; j++)
{
jj=i*l+j;
a[jj]=0.0;
for (kk=0; kk<=n-1; kk++)
{
a[jj]=a[jj]+b[i*l+kk]*f[j*n+kk];
}
}
}
for (i=0; i<=n-1; i++)
{
for (j=0; j<=n-1; j++)
{
jj=i*n+j;
p[jj]=q[jj];
for (kk=0; kk<=n-1; kk++)
{
p[jj]=p[jj]+f[i*n+kk]*a[j*l+kk];
}
}
}
}
}
free(e); free(a); free(b);
return(js);
}
int brinv(double a[],int n)
{
int *is,*js,i,j,k,l,u,v;
double d,p;
is=(int*)malloc(n*sizeof(int));
js=(int*)malloc(n*sizeof(int));
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)
{
free(is);
free(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;
}
}
}
free(is); free(js);
return(1);
}
