wiener.c

Wiener滤波器（维纳滤波器）的C程序

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

#define N 1024
#define PI 3.14159
  
double tz(double a,double b,long int *c);
double gauss(double mn,double sgm,long int *s);
double exponent(double bt ,long int *s);
void levin(double t[],double b[],int n,double x[]);
void wiener(double rxx[],double rdx[],int p,double h[],double *e,double x[],double y[],int n);
void my_sin(double a[]);

void main()
{
	int i,j,p;
	double mn = 0.0;
	double sgm = 1.0;
	long int s = 13278;
	double e;
	double signal[N],noise[N],sig_n[N],sig_wiener[N];
    double rxx[30],rdx[30],h[30];
	static double a[3]={1.0,-0.1,-0.8};
 	static double b[1]={1.0};

  	FILE *fp1=fopen("sig.txt","w");
  	FILE *fp2=fopen("sig_n.txt","w");
	FILE *fp3=fopen("sig_w.txt","w");
  
 	for(i = 0 ; i < N ; i++)
	{
	   my_sin(signal);
 	   noise[i] = gauss(mn, sgm,&s);
 	}
    for(i = 0 ; i < N ; i++)
	{
 	   sig_n[i] = signal[i] + noise[i];
 	}
    p = 8;
	for(i = 0 ; i <= p ; i++)
	{
 	   rdx[i] = 0.0;
	   for(j = 0 ; j < (N-i) ; j++)
	   {
		   rdx[i] += signal[j] * signal[j+i];
	   }
	   rdx[i] = rdx[i]/N;
 	}
	rxx[0] = rdx[0] + 1.0;
	for(i = 1 ; i <= p ; i++)
	{
 	   rxx[i] = rdx[i];
 	}
	wiener(rxx, rdx, p, h, &e, sig_n, sig_wiener, N);
	for(i = 0 ; i < N ; i++)
	{
		fprintf(fp1,"%10.8f\n",signal[i]);
		fprintf(fp2,"%10.8f\n",sig_n[i]);
		fprintf(fp3,"%10.8f\n",sig_wiener[i]);
    }
    fclose(fp1);
	fclose(fp2);
	fclose(fp3);
}

void my_sin(double a[])
{
	int i;
	for(i=0 ; i<N ; i++)
	{
		a[i] = 5 * sin(2*PI*i/100);
	}

}

double tz(double a,double b,long int *c)
{
    *c = 2045 * (*c) + 1;
    *c = *c - (*c / 1048576) * 1048576;
    return(a + (b - a) * ((*c) / (double)1048576));
}

double gauss(double mn,double sgm,long int *s)
{
    int i;
	double x,y;
	double tz();
	for(x = 0, i = 0 ; i < 12 ; i++)
	{
		x += tz(0.0, 1.0, s);	
	}
	x = x - 6.0;
	y = mn + x * sgm;
    return(y);
}

double exponent(double bt ,long int *s)
{
    double u,x;
    double tz();
    u=tz(0.0,1.0,s);
    x=-bt * log(u);
    return(x);
}

void levin(double t[],double b[],int n,double x[])
{
	int i,j,k;
	double a,bt,q,c,h,*y,*s;
	s = malloc(n * sizeof(double));
	y = malloc(n * sizeof(double));
	a = t[0];
	y[0] = 1.0;
	x[0] = b[0] / a;
	for(k=1;k<n;k++)
	{
		bt = 0.0;
		q = 0.0;
		for(j = 0 ; j < k ; j++)
		{
			bt += y[j] * t[j+1];
			q += x[j] * t[k-j];
		}
		c = -bt / a;
		s[0] = c * y[k-1];
        y[k] = y[k-1];
		if(k != 1)
		{
		    for(i = 1 ; i < k ; i++)
			{
			    s[i] = y[i-1] + c * y[k-i-1];
			}
		}
		s[k] = y[k-1];
		a += c * bt;
		h = (b[k] - q) / a;
		for(i = 0 ; i < k ; i ++)
		{
			x[i] += h * s[i];
		    y[i] = s[i];
		}
		x[k] = h * y[k];
	}
	free(s);
	free(y);
}

void wiener(double rxx[],double rdx[],int p,double h[],double *e,double x[],double y[],int n)
{
	int i,k;
	double sum;
	levin(rxx, rdx, p+1, h);
	sum = 0.0;
	for(i = 0 ; i <= p ; i++)
	{
		sum += rdx[i] * h[i];
	}
	*e = rdx[0] - sum;
	for(k = 0 ; k < n ; k++)
	{
		y[k] = 0.0;
		for(i = 0 ; i <= p ; i++)
		{
			if((k - i)>=0)
			{
				y[k] += h[i] * x[k-i];
			}
		}
	}
}