数值计算程序大放送-数学变换与滤波

数值计算程序大放送-数学变换与滤波

//////////////////////////////////////////////////////////////
//傅里叶级数逼近
//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[]);
//////////////////////////////////////////////////////////////

 

选自<<徐世良数值计算程序集(C)>>

每个程序都加上了适当地注释，陆陆续续干了几个月才整理出来的啊。

今天都给贴出来了

#include

//傅里叶级数逼近
//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[])
{ int i,j;
    double t,c,s,c1,s1,u1,u2,u0;
    t=6.283185306/(2.0*n+1.0);
    c=cos(t);
s=sin(t);
    t=2.0/(2.0*n+1.0);
c1=1.0;
s1=0.0;
    for (i=0; i<=n; i++)
{
  u1=0.0; u2=0.0;
        for (j=2*n; j>=1; j--)
  {
   u0=f[j]+2.0*c1*u1-u2;
            u2=u1; u1=u0;
  }
        a[i]=t*(f[0]+u1*c1-u2);
        b[i]=t*u1*s1;
        u0=c*c1-s*s1;
  s1=c*s1+s*c1;
  c1=u0;
}
    return;
}

 

//快速离散傅里叶变换（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);
}