C语言滤波器实现，有FIR的低通、高通、带通、带阻等，卡尔曼滤波器

//离散随机线性系统的卡尔曼滤波 #include "stdio.h" #include "math.h" #include "stdlib.h" int klman(int n,int m,int k,double f[],double q[], double r[], double h[],double y[],double x[],double p[],double g[]); static void keklm5(int n,double y[]); int brinv(double a[],int n); main() { int i,j,js; void keklm5(); double p[3][3],x[200][3],y[200][1],g[3][1],t,s; static double f[3][3]={{1.0,0.05,0.00125}, {0.0,1.0,0.05}, {0.0,0.0,1.0}}; static double q[3][3]={{0.25,0.0,0.0}, {0.0,0.25,0.0}, {0.0,0.0,0.25}}; static double r[1][1]={{0.25}}; static double h[1][3]={{1.0,0.0,0.0}}; for (i=0; i<=2; i++) for (j=0; j<=2; j++) p[i][j]=0.0; for (i=0; i<=199; i++) for (j=0; j<=2; j++) x[i][j]=0.0; keklm5(200,*y); //产生长度200的高斯序列 for (i=0; i<=199; i++) { t=0.05*i; y[i][0]=5.0-2.0*t+3.0*t*t+y[i][0]; } js=klman(3,1,200,*f,*q,*r,*h,*y,*x,*p,*g); //卡尔曼滤波 if (js!=0) { printf("\n"); printf(" t s y "); printf("x(0) x[1] x(2) \n"); for (i=0; i<=199; i=i+5) { t=0.05*i; s=5.0-2.0*t+3.0*t*t; printf("%6.2f %e %e %e %e %e\n", t,s,y[i][0],x[i][0],x[i][1],x[i][2]); } printf("\ninput i:"); scanf("%d",i); } return; } static void keklm5(int n,double y[]) //产生均值为0,方差为0.5^2的高斯白噪声序列 { int i,j,m; //n为序列长度,y[]返回序列的首地址 double s,w,v,r,t; s=65536.0; w=2053.0; v=13849.0; r=0.0; for (i=0; i<=n-1; i++) { t=0.0; for (j=0; j<=11; j++) { r=w*r+v; m=r/s; r=r-m*s; t=t+r/s;} y[i]=0.5*(t-6.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=malloc(m*m*sizeof(double)); l=m; if (l<n) l=n; a=malloc(l*l*sizeof(double)); b=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); //矩阵e求e_1 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); } int brinv(double a[],int n) //实矩阵A求A_1 { int *is,*js,i,j,k,l,u,v; //函数返回标志值,若返回0,则矩阵奇异 double d,p; //若返回不为0,则表示正常返回 is=malloc(n*sizeof(int)); //a双精度二维数组,n*n存放原矩阵,返回A_1 js=malloc(n*sizeof(int)); //n矩阵a的阶数 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); }