dingdian.c

分组滤波器的C语言源码

 -18284, -18002, -17709, -17498, -17223, -16890, -16638, -16319,
 -16027, -15742, -15489, -15171, -14890, -14655, -14302, -14025,
 -13777, -13522, -13223, -12944, -12686, -12417, -12136, -11855,
 -11592, -11355, -11022, -10772, -10518, -10213,  -9898,  -9645,
  -9365,  -9032,  -8797,  -8428,  -8160,  -7848,  -7527,  -7160,
  -6865,  -6502,  -6166,  -5849,  -5454,  -5118,  -4691,  -4343,
  -3978,  -3598,  -3168,  -2795,  -2346,  -1913,  -1464,  -1035,
   -594,   -138,    301,    745,   1215,   1743,   2210,   2651,
   3214,   3704,   4183,   4727,   5228,   5743,   6292,   6839,
   7328,   7884,   8443,   8983,   9529,  10078,  10621,  11126,
  11709,  12241,  12745,  13294,  13822,  14369,  14833,  15338,
  15858,  16327,  16792,  17241,  17682,  18089,  18504,  18866,
  19248,  19597,  19891,  20199,  20474,  20739,  20959,  21181,
  21351,  21483,  21585,  21768,  21856,  21912,  21919,  21886,
  21909,  21891,  21805,  21745,  21659,  21489,  21362,  21160,
  21052,  20863,  20681,  20439,  20192,  19963,  19715,  19470,
  19225,  18938,  18673,  18368,  18078,  17816,  17545,  17249,
  16935,  16702,  16406,  16098,  15815,  15536,  15282,  14946,
  14705,  14430,  14128,  13872,  13583,  13340,  13070,  12765,
  12501,  12253,  11979,  11671,  11441,  11181,  10847,  10579,
  10304,   9992,   9669,   9341,   9065,   8787,   8494,   8140,
   7792,   7421,   7104,   6760,   6384,   6012,   5685,   5260,
   4891,   4518,   4065,   3666,   3272,   2857,   2387,   1992,
   1506,   1109,    627,    162,   -319,   -806,  -1260,  -1777,
  -2302,  -2785,  -3279,  -3796,  -4323,  -4782,  -5371,  -5894,
  -6399,  -6972,  -7541,  -8078,  -8612,  -9114,  -9678, -10222,
 -10782, -11304, -11845, -12358, -12901, -13430, -13913, -14443,
 -14895, -15368, -15867, -16328, -16733, -17154, -17582, -17947,
 -18331, -18623, -18988, -19284, -19548, -19805, -20049, -20260,
 -20413, -20602, -20735, -20816, -20907, -20947, -21007, -21022,
 -20956, -20945, -20931, -20774, -20706, -20589, -20470, -20292,
 -20080, -19952, -19757, -19517, -19263, -19065, -18801, -18530,
 -18288, -18033, -17753, -17443, -17195, -16934, -16621, -16363,
 -16050, -15731, -15508, -15188, -14905, -14607, -14338, -14021,
 -13771, -13501, -13214, -12941, -12655, -12403, -12167, -11864,
 -11636, -11299, -11046, -10763, -10490, -10216,  -9937,  -9667,
  -9354,  -9074,  -8775,  -8494,  -8196,  -7844,  -7542,  -7201,
  -6881,  -6523,  -6203,  -5837,  -5489,  -5088,  -4758,  -4360,
  -3996,  -3601,  -3219,  -2723,  -2369,  -1925,  -1479,  -1056,
   -652,   -192,    285,    753,   1235,   1671,   2162,   2668,
   3162,   3689,   4207,   4679,   5217,   5762,   6266,   6814,
   7356,   7915,   8420,   8960,   9487,  10063,  10608,  11169,
  11748,  12271,  12758,  13297,  13812,  14353,  14797,  15389,
  15858,  16297,  16771,  17228,  17663,  18074,  18451,  18858,
  19238,  19531,  19907,  20231,  20471,  20736,  20974,  21123,
  21360,  21514,  21625,  21752,  21803,  21869,  21938,  21912,
  21881,  21880,  21809,  21734,  21613,  21506,  21386,  21219,
  21013,  20852,  20656,  20447,  20221,  19986,  19745,  19442,
  19240,  18903,  18652,  18415,  18077,  17835,  17538,  17259,
  16972,  16675,  16379,  16130,  15811,  15568,  15262,  15015,
  14713,  14460,  14117,  13911,  13629,  13328,  13063,  12822,
  12472,  12262,  11976,  11751,  11446,  11113,  10871,  10599,
  10283,   9964,   9669,   9391,   9059,   8784,   8429,   8128,
   7785,   7471,   7118,   6769,   6426,   6029,   5682,   5266,
   4887,   4489,   4131,   3683,   3288,   2856,   2442,   1965,
   1547,   1104,    632,    191,   -287,   -768,  -1261,  -1756,
  -2269,  -2753,  -3279,  -3802,  -4272,  -4802,  -5380,  -5884,
  -6418,  -6963,  -7496,  -8080,  -8611,  -9114,  -9634, -10234,
 -10772, -11298, -11830, -12380, -12894, -13412, -13925, -14425,
 -14893, -15388, -15822, -16302, -16700, -17154, -17558, -17944,
 -18296, -18627, -18960, -19246, -19519, -19819, -20025, -20242,
 -20438, -20565, -20705, -20814, -20884, -20958, -21007, -20999,
 -21014, -20969, -20894, -20804, -20710, -20602, -20443, -20293,
 -20093, -19915, -19755, -19539, -19303, -19056, -18843, -18557,
 -18286, -18019, -17743, -17512, -17201, -16908, -16628, -16346,
 -16061, -15733, -15486, -15163, -14908, -14611, -14347, -14079,
 -13784, -13540, -13249, -12962, -12726, -12448, -12175, -11901,
 -11609, -11334, -11059, -10769, -10498, -10204,  -9940,  -9658,
  -9344,  -9069,  -8796,  -8503,  -8204,  -7899,  -7538,  -7220,
  -6877,  -6507,  -6192,  -5844,  -5485,  -5128,  -4729,  -4343,
  -3965,  -3569,  -3206,  -2779,  -2382,  -1955,  -1526,  -1048,
   -629,   -175,    273,    724,   1184,   1703,   2188,   2632,
   3162,   3650,   4147,   4691,   5185,   5723,   6279,   6759,
   7338,   7856,   8445,   8934,   9500,   9992,  10604,  11129,
  11666,  12175,  12722,  13298,  13814,  14298,  14817,  15319,
  15832,  16234,  16752,  17201,  17634,  18084,  18463,  18884,
  19225,  19567,  19938,  20192,  20446,  20693,  20963,  21179,
  21315,  21486,  21652,  21731,  21796,  21880,  21903,  21927,
  21917,  21863,  21786,  21781,  21607,  21492,  21371,  21184,
  21056,  20895,  20644,  20458,  20201,  19996,  19762,  19466,
  19216,  18921,  18692,  18381,  18102,  17812,  17557,  17251,
  16996,  16678,  16379,  16088,  15831,  15558,  15276,  14968,
  14736,  14447,  14183,  13905,  13579,  13333,  13084,  12803,
  12521,  12206,  12016,  11735,  11452,  11195,  10855,  10574,
  10281,  10012,   9689,   9398,   9097,   8745,   8466,   8135,
   7844,   7454,   7126,   6755,   6419,   6045,   5646,   5278,
   4904,   4496,   4136,   3668,   3322,   2841,   2466,   2024,
   1562,   1089,    671,    180,   -280,   -806,  -1256,  -1753,
  -2206,  -2711,  -3256,  -3724,  -4321,  -4772,  -5346,  -5857,
  -6417,  -6947,  -7478,  -8023,  -8534,  -9081,  -9643, -10208,
 -10735, -11310, -11788, -12354, -12863, -13368, -13897, -14406,
 -14888, -15369, -15836, -16273, -16722, -17194, -17563, -17941,
 -18296, -18632, -18953, -19304, -19546, -19795, -19992, -20229,
 -20405, -20562, -20716, -20787, -20920, -20969, -21000, -20992,
 -20957, -20962, -20925, -20791, -20693, -20599, -20442, -20309,
 -20113, -19954, -19759, -19493, -19318, -19093, -18817, -18558,
 -18295, -18011, -17743, -17465, -17219, -16902, -16652, -16355,
 -16064, -15776, -15488, -15206, -14943, -14613, -14361, -14080,
 -13796, -13548, -13253, -12993, -12698, -12463, -12157, -11867,
 -11627, -11353, -11073, -10782, -10537, -10206,  -9995,  -9673,
  -9385,  -9075,  -8763,  -8463,  -8209,  -7861,  -7547,  -7234,
  -6888,  -6506,  -6223,  -5827,  -5498,  -5172,  -4777,  -4417,
  -4013,  -3616,  -3183,  -2788,  -2355,  -1982,  -1542,  -1090,
   -638,   -182,    236,    687,   1227,   1671,   2192,   2656,
   3134,   3650,   4148,   4718,   5192,   5716,   6256,   6812,
   7331,   7853,   8417,   8931,   9456,  10040,  10545,  11113,
  11619,  12185,  12711,  13248,  13752,  14292,  14765,  15320,
  15796,  16310,  16760,  17207,  17661,  18090,  18483,  18822,
  19208,  19550,  19864,  20181,  20445,  20678,  20974,  21117,
  21324,  21498,  21603,  21734,  21805,  21903,  21884,  21926,
  21870,  21875,  21828,  21724,  21634,  21523,  21354,  21241,
  21060,  20867,  20673,  20438,  20176,  19941,  19757,  19490,
  19221,  18961,  18697,  18367,  18171,  17855,  17546,  17283,
  16975,  16692,  16381,  16145,  15859,  15554,  15296,  15027,
  14724,  14465,  14189,  13913,  13632,  13331,  13073,  12758,
  12510,  12265,  11983,  11724,  11479,  11143,  10847,  10610,
  10333,  10013,   9722,   9387,   9086,   8803,   8471,   8137,
   7840,   7484,   7131,   6777,   6452,   6033,   5638,   5302,
   4915,   4547,   4137,   3716,   3279,   2852,   2465,   1996,
   1572,   1087,    695,    181,   -288,   -744,  -1212,  -1741,
  -2203,  -2714,  -3234,  -3737,  -4223,  -4787,  -5285,  -5835,
  -6361,  -6934,  -7475,  -7993,  -8533,  -9065,  -9679, -10228,
 -10723, -11271, -11817, -12306, -12825, -13364, -13850, -14340,
 -14862, -15361, -15805, -16256, -16738, -17110, -17508, -17914,
 -18296, -18625, -18962, -19248, -19525, -19775, -20031, -20244,
 -20394, -20546, -20697, -20816, -20886, -20949, -21007, -20996,
 -20981, -20962, -20889, -20816, -20736, -20579, -20445, -20299,
 -20126, -19950, -19774, -19535, -19287, -19093, -18802, -18541,
 -18300, -18028, -17833, -17487, -17216, -16922, -16635, -16338,
 -16058, -15753, -15529, -15176, -14944, -14643, -14347, -14107,
 -13809, -13493, -13278, -12951, -12697, -12492, -12134, -11891,
 -11591, -11334, -11071, -10764, -10543, -10220,  -9987,  -9645,
  -9404,  -9092,  -8780,  -8482,  -8227,  -7846,  -7533,  -7235,
  -6909,  -6548,  -6227,  -5877,  -5531,  -5097,  -4793,  -4375,
  -3984,  -3614,  -3214,  -2786,  -2393,  -1983,  -1584,  -1114};




 double ans;
 main()
{
const double pi=3.141592653589793;
 int i;
 
 
 Cmf_Window(Hamming_Window,data_l);
 Cmf_Window(Hamming_Window,data_r);

 Cmf_Group_Fir(Fir_Filter,data_l);
 Cmf_Group_Fir(Fir_Filter,data_r);

 //Cmf_Window(Hamming_Window,data_l);
 //Cmf_Window(Hamming_Window,data_r);

 Cmf_Sgolayfilt(data_l);
 Cmf_Sgolayfilt(data_r);

 ans=Cmf_Get_Phase_dif(data_l,data_r);

}


void Cmf_Window(const long p_window[],long p_data[])
{
	int n;
	for(n=0;n<2048;n++)
	{
		p_data[n]=(p_data[n]*p_window[n])>>15;	
	}
}

void Cmf_Group_Fir(const long p_Fir[],long p_data[])
{
	long cx[64];
	int count,count1;
	for(count=0;count<32;count++)
		 {
			 for(count1=0;count1<64;count1++)
				 	 {
				 		 cx[count1]=p_data[count+32*count1];
					 }
			 
			 Cmf_Fir(p_Fir,cx);
	 
			 for(count1=0;count1<64;count1++)
				 {
					p_data[count+32*count1]=cx[count1];
				 }
		}
}



void Cmf_Fir(const long p_Fir[],long p_data[])
{
	int i,j;
	long data[64];
	for(i=0;i<64;i++)
	{
		data[i]=0;
		for(j=0;j<64;j++)
		{
			data[i]=p_data[j]*p_Fir[i+j+1]+data[i];
		}
	}
	for(i=0;i<64;i++)
	{
		p_data[63-i]=data[i]>>18;
	}

}


void Cmf_Sgolayfilt(long p_data[])
{
	int s[9]={-63,42,117,162,177,162,117,42,-63};
	int i;
	long data_save[5];
		data_save[0]=0;
		data_save[1]=p_data[0];
		data_save[2]=p_data[1];
		data_save[3]=p_data[2];
		data_save[4]=p_data[3];
	for(i=4;i<2044;i++)
	{
		data_save[0]=data_save[1];
		data_save[1]=data_save[2];
		data_save[2]=data_save[3];
		data_save[3]=data_save[4];
		data_save[4]=p_data[i];
		p_data[i]=(data_save[0]*s[0]+data_save[1]*s[1]+data_save[2]*s[2]+data_save[3]*s[3]+data_save[4]*s[4]+p_data[i+1]*s[5]+p_data[i+2]*s[6]+p_data[i+3]*s[7]+p_data[i+4]*s[8])/693;
	}
	p_data[0]=0;
	p_data[1]=0;
	p_data[2]=0;
	p_data[3]=0;
	p_data[2044]=0;
	p_data[2045]=0;
	p_data[2046]=0;
	p_data[2047]=0;

}



double Cmf_Get_Phase_dif(long p_ldata[],long p_rdata[])
{
	double sum1,sum2,sum3;
	double ans;
	int i;
	sum1=0;
	sum2=0;
	sum3=0;
	for(i=4;i<2044;i++)
	{
		sum1=p_ldata[i]*p_rdata[i]+sum1;
		sum2=p_ldata[i]*p_ldata[i]+sum2;
		sum3=p_rdata[i]*p_rdata[i]+sum3;
	}
	ans=acos(sum1/sqrt(sum2*sum3));
	return(ans);
}