notchfilter1.c

抑制单频干扰陷波器

/*---------------------------------------------------------

	Notch Filter:
	B:			input data(complex, frequency domain)
	len_B:		complex data length

  	Copyrights Innofidei, Inc. Beijing. All rights reserved.
	Version 0.1

	Confidential, do not release this program package to anyone without permission.
	Current responsible engineer: Shen Yebing.
	Supervisor of the responsible engineer: Kang Liangchuan.
	When this file is modified, inform Kang Liangchuan, Shao Zhihui, Zhang Yubing, Shen Yebing.	
	Revision history:
	2007-07-20	Created by Kang Liangchuan.
---------------------------------------------------------*/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <malloc.h>

void notchfilter1(double *B,int len_B,double pnotch)
{
	int i,j;
	double Esav=0;
	double Gnotch;
	double Etemp=0;
	double Emax=0;
	static double *A1;

	static int counter;
	A1=(double *)calloc(len_B*8,sizeof(double));
   
	//double pnotch = 4.0;

	for(i=0; i<len_B; i++)
	{   for(j=0;j<3;j++)
	    A1[j*len_B+i]= A1[(j+1)*len_B+i];
		A1[3*len_B+i]=(B[2*i]*B[2*i] + B[2*i+1]*B[2*i+1]);
		Etemp=0;
	    for(j=0;j<4;j++)
		{Esav = Esav +A1[j*len_B+i];

	     Etemp=Etemp+A1[j*len_B+i];
		}
		if(Emax<Etemp)
        {Emax=Etemp;
		 
		}
		
	}
	Esav = Esav/len_B;
 

	counter++;
	if(counter<4)
	{
	 if(sqrt(Emax/Esav)>6.0)
	 { Gnotch =sqrt(Esav/counter)*4.0;
	 
	 }
     /*else if(sqrt(Emax/Esav)>7)
	 { Gnotch =sqrt(Esav/counter)*7;
	 
	 }
	   else if(sqrt(Emax/Esav)>6)
	 { Gnotch =sqrt(Esav/counter)*6;
	 
	 }*/
  
	else
	 Gnotch =sqrt(Esav/counter)*1000000.0;
	}
	else
	{
		counter=0;
		if(sqrt(Emax/Esav)>6.0)
	 { Gnotch =sqrt(Esav/4)*4.0;
	 
	 }
    /* else if(sqrt(Emax/Esav)>7)
	 { Gnotch =sqrt(Esav/4)*7;
	 
	 }
	 else if(sqrt(Emax/Esav)>6)
	 { Gnotch =sqrt(Esav/4)*6;
	 
	 }*/
  
	else
	 Gnotch =sqrt(Esav/4)*1000000.0;
	}

	for (i=0; i<len_B; i++)
	{
		if(sqrt(B[2*i]*B[2*i]+B[2*i+1]*B[2*i+1]) > Gnotch)
		{  
	        B[2*i] =0;
			B[2*i+1] =0;	
		
			
		}
	
	}
}