数学变换与滤波

2019独角兽企业重金招聘Python工程师标准>>>

#include "stdafx.h"
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "MatrixAlgo.h"
#include "TransformFilter.h"

//快速沃什变换
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 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;
}

//离散随机线性系统的卡尔曼滤波
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<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;
}

//五点三次平滑
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;
}

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

                                     ----根据《C语言常用算法程序集》整理

 

投票>

转载于:https://my.oschina.net/RapidBird/blog/3524