ch11.h

数值处理算法程序

/************************************************
 Expect bugs!
 Please use and enjoy, and let me know of any bugs/mods/improvements 
 that you have found/implemented and I will fix/incorporate them into 
 this file. Thank Mr. Xushiliang once again.

					hujinshan@2002.11.3
				Airforce Engineering University
************************************************/

/*****  #include "CH11.h"  数学变换与滤波 *****/
#ifndef CH11_H_
#define CH11_H_

#include "stdlib.h"
#include "math.h"
#include "stdio.h"

#include "ch2.h"
//*******************************************************************

void kfour(double f[],int n,double a[],double b[]);//傅里叶级数逼近
void kkfft(double pr[],double pi[],int n,int k,double fr[],double fi[],int l,int il);//快速傅里叶变换
void kkfwt(double p[],int n,int k,double x[]);//快速沃什变换
void kkspt(int n,double y[],double yy[]);//五点三次平滑
int klman(int n,int m,int k,double f[],double q[],double r[],double h[],double y[],double x[],double p[],double g[]);//离散随机线性系统卡尔曼滤波
void kkabg(int n,double x[],double t,double a,double b,double c,double y[]);//α-β-γ滤波

//*******************************************************************
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;
}
/////////////////////////////////////////////////////////////
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;
}
/////////////////////////////////////////////////////////////
void kkfwt(double p[],int n,int k,double x[])
{ 
	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;
}
/////////////////////////////////////////////////////////////
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;
}
/////////////////////////////////////////////////////////////
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;
    //extern int brinv();
    e=(double*)malloc(m*m*sizeof(double));
    l=m;
    if (l<n) 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<k)
          { 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);
}
/////////////////////////////////////////////////////////////
void kkabg(int n,double x[],double t,double a,double b,double c,double y[])
{ 
	int i;
    double s1,ss,v1,vv,a1,aa;
    aa=0.0; vv=0.0;ss=0.0;
    for (i=0; i<=n-1; i++)
    { 
		s1=ss+t*vv+t*t*aa/2.0;
        v1=vv+t*aa; a1=aa;
        ss=s1+a*(x[i]-s1); y[i]=ss;
        vv=v1+b*(x[i]-s1);
        aa=a1+2.0*c*(x[i]-s1)/(t*t);
	}
    return;
}
/////////////////////////////////////////////////////////////
#endif