gyroscope_sdlg.cpp

中值滤波加提升小波降噪方法

						n++;
						UpdateData(FALSE);	
						
					}//else
					break;
			case 1:
				if(m_num<32)
					{
						m_xGyro=str.Mid(1,6);
						m_xJiaG=str.Mid(7,6);
						m_xTemp=str.Mid(13,6);
						m_xBenchmark=str.Mid(19,6);

						m_yGyro=str.Mid(25,6);
						m_yJiaG=str.Mid(31,6);
						m_yTemp=str.Mid(37,6);
						m_yBenchmark=str.Mid(43,6);

						extern float gyroscope1[32],gyroscope2[32];
						gyroscope1[m_num]=atof(m_xGyro);
						gyroscope2[m_num]=atof(m_yGyro);
						
						m_num++;
						UpdateData(FALSE);
					}
				else
					{  
						m_xGyro=str.Mid(1,6);
						m_xJiaG=str.Mid(7,6);
						m_xTemp=str.Mid(13,6);
						m_xBenchmark=str.Mid(19,6);

						m_yGyro=str.Mid(25,6);
						m_yJiaG=str.Mid(31,6);
						m_yTemp=str.Mid(37,6);
						m_yBenchmark=str.Mid(43,6);

						
						fxGyro=atof(m_xGyro);
						fyGyro=atof(m_yGyro);
			
						extern float gyroscope1[32],gyroscope2[32];
						SYSTEMTIME t1,t2;
						GetSystemTime(&t1);               
						op1=TI_Denoise(gyroscope1);
						op2=TI_Denoise(gyroscope2);
						GetSystemTime(&t2);
						int ms;
						ms=t2.wMilliseconds-t1.wMilliseconds;
						strTime.Format("%d",ms);
						strT_xGyro.Format("%f",op1);
						strT_yGyro.Format("%f",op2);
						if(n<10)
						{
							m_xSourceSignal+=m_xGyro;
							m_xSourceSignal+="\r\n";
							m_xDenoiseSignal+=strT_xGyro;
							m_xDenoiseSignal+="\r\n";

							m_ySourceSignal+=m_yGyro;
							m_ySourceSignal+="\r\n";
							m_yDenoiseSignal+=strT_yGyro;
							m_yDenoiseSignal+="\r\n";

							m_time+=strTime;
							m_time+="\r\n";
						}
						else
						{
							m_xSourceSignal.Empty();
							m_xDenoiseSignal.Empty();
							m_xSourceSignal+=m_xGyro;
							m_xSourceSignal+="\r\n";
							m_xDenoiseSignal+=strT_xGyro;
							m_xDenoiseSignal+="\r\n";

							m_ySourceSignal.Empty();
					        m_yDenoiseSignal.Empty();
					        m_ySourceSignal+=m_yGyro;
					        m_ySourceSignal+="\r\n";
					        m_yDenoiseSignal+=strT_yGyro;
						    m_yDenoiseSignal+="\r\n";


							m_time.Empty();
							m_time+=strTime;
							m_time+="\r\n";
							n=0;
						}

						for(int j=0;j<31;j++)
						{	gyroscope1[j]=gyroscope1[j+1];
						    gyroscope2[j]=gyroscope2[j+1];
						}
						gyroscope1[31]=fxGyro;
						gyroscope2[31]=fyGyro;
						n++;
						UpdateData(FALSE);	
						
					}//else
					break;
			case 0:
				if(m_num<32)
					{
						m_xGyro=str.Mid(1,6);
						m_xJiaG=str.Mid(7,6);
						m_xTemp=str.Mid(13,6);
						m_xBenchmark=str.Mid(19,6);

						extern float gyroscope1[32];
						gyroscope1[m_num]=atof(m_xGyro);					
						
						m_num++;
						UpdateData(FALSE);
					}
				else
					{  
						m_xGyro=str.Mid(1,6);
						m_xJiaG=str.Mid(7,6);
						m_xTemp=str.Mid(13,6);
						m_xBenchmark=str.Mid(19,6);

						fxGyro=atof(m_xGyro);
			
						extern float gyroscope1[32];
						SYSTEMTIME t1,t2;
						GetSystemTime(&t1);               
						op1=TI_Denoise(gyroscope1);	
						GetSystemTime(&t2);
						int ms;
						ms=t2.wMilliseconds-t1.wMilliseconds;
						strTime.Format("%d",ms);
						strT_xGyro.Format("%f",op1);

						if(n<10)
						{
							m_xSourceSignal+=m_xGyro;
							m_xSourceSignal+="\r\n";
							m_xDenoiseSignal+=strT_xGyro;
							m_xDenoiseSignal+="\r\n";

							m_time+=strTime;
							m_time+="\r\n";
						}
						else
						{
							m_xSourceSignal.Empty();
							m_xDenoiseSignal.Empty();
							m_xSourceSignal+=m_xGyro;
							m_xSourceSignal+="\r\n";
							m_xDenoiseSignal+=strT_xGyro;
							m_xDenoiseSignal+="\r\n";

							m_time.Empty();
							m_time+=strTime;
							m_time+="\r\n";
							n=0;
						}

						for(int j=0;j<31;j++)
						{	
							gyroscope1[j]=gyroscope1[j+1];
						}
						gyroscope1[31]=fxGyro;
						n++;
						UpdateData(FALSE);	
						
					}//else
					break;
					
		}//switch
	}

}



float CGyroscope_sDlg::TI_Denoise(float signal[])
{
	int i,j,m,size=0;
	//初始化table[N][L+1]
	for(i=0;i<32;i++)
	{
		for(j=0;j<=4;j++)
		{
			if(0==j)
				table[i][j]=signal[i];
			else
				table[i][j]=0;
		}
	}

	//return table[31][0];
    //dwt
	
	for(i=1;i<=4;i++)
	{
		size=(32>>i);
		for(j=0;j<=(1<<i)-2;j=j+2)
		{
			for(m=0;m<size*2;m++)
			{
				signal[m]=table[size*j+m][0];
				temp[m]=table[size*j+m][0];
			}
            db4lift_dwt(signal,size);
			
			for(m=0;m<size;m++)
			{
				table[m+size*j][0]=signal[2*m];
				table[m+size*j][i]=signal[2*m+1];
			}

			db4lift_dwt(temp,size);

			for(m=0;m<size;m++)
			{
				table[(j+1)*size+m][0]=temp[2*m];
				table[(j+1)*size+m][i]=temp[2*m+1];
			}
			
		}
	}
	
	//Soft threshold denoising
	thresh_soft(table);

	//idwt
	for(i=4;i>=1;i--)
	{
		size=(32>>i);
		for(j=0;j<=(1<<i)-2;j=j+2)
		{
			for(m=0;m<size;m++)
			{
				signal[m]=table[j*size+m][0];
				signal[size+m]=table[j*size+m][i];
			}
			
			db4lift_idwt(signal,size);
            iadjust(signal,size);
			for(m=0;m<size;m++)
			{
				temp[m]=table[(j+1)*size+m][0];
				temp[size+m]=table[(j+1)*size+m][i];
			}

            db4lift_idwt(temp,size);

            iadjust(temp,size);

			for(m=0;m<size*2;m++)
			{
				table[size*j+m][0]=(signal[m]+temp[m])/2;
			}
		}
	}

	for(i=0;i<32;i++)
	{
		output[i]=table[i][0];
	}
	//return output[31];
	float outputR;
    outputR= flp(output);
	return outputR;

}

void CGyroscope_sDlg::db4lift_dwt(float processing[], int n)
{
	int i;
    for(i=0;i<n;i++)
	{
		processing[2*i]=processing[2*i]+1.732*processing[2*i+1];
	}

	for(i=0;i<n;i++)
	{
		if(0==i)
			processing[2*i+1]=processing[2*i+1]-0.433*processing[2*i]+0.067*processing[2*i+2];//
		else 
			processing[2*i+1]=processing[2*i+1]-0.433*processing[2*i]+0.067*processing[2*i-2];
	}

	for(i=0;i<n;i++)
	{
		if((n-1)!=i)
			processing[2*i]=processing[2*i]-processing[2*i+1+2];
		else
			processing[2*i]=processing[2*i]-processing[2*i+1-2];//
	}

	for(i=0;i<n;i++)
	{
		processing[2*i]=processing[2*i]*0.5176;
		processing[2*i+1]=processing[2*i+1]*1.932;
	}

}

void CGyroscope_sDlg::db4lift_idwt(float processing[], int n)
{
	int i;

	for(i=0;i<n;i++)
	{
		processing[i]=processing[i]/0.5176;
		processing[n+i]=processing[n+i]/1.932;
	}

	for(i=0;i<n;i++)
	{
		if((n-1)!=i)
			processing[i]=processing[i]+processing[n+i+1];
		else
			processing[i]=processing[i]+processing[n+i-1];//
	}

	for(i=0;i<n;i++)
	{
		if(0==i)
			processing[n+i]=processing[n+i]+0.433*processing[i]-0.067*processing[i+1];//
		else
			processing[n+i]=processing[n+i]+0.433*processing[i]-0.067*processing[i-1];

	}

	for(i=0;i<n;i++)
	{
		processing[i]=processing[i]-1.732*processing[n+i];
	}
}

void CGyroscope_sDlg::iadjust(float processing[], int n)
{
	int i;
	float t[32];
    for(i=0;i<n;i++)
	{
		t[i]=processing[n+i];
	}

	for(i=n-1;i>=0;i--)
	{
		
		processing[2*i]=processing[i];
		
	}

	for(i=0;i<n;i++)
	{
		processing[2*i+1]=t[i];
	}

}

void CGyroscope_sDlg::thresh_soft(float table[32][5])
{
	int i,j;
	float u;
	float thresh,sigma;
	//if(table>0)
//	{table=table;}
//	else
	//{table=-table;}
	
	sigma=median(table)/0.6745;
    
	for(i=1;i<=3;i++)
	{
		for(j=0;j<32;j++)
		{
		thresh=(3.6*sigma)/(log(j+1));//sqrt(2*log(N))*sigma;
        u=1/((table[j][i]-thresh)*(table[j][i]-thresh)+1);
			if(myfabs(table[j][i])<thresh)
				table[j][i]=0;
			else if(table[j][i]>=thresh)
				table[j][i]=table[j][i]-u*thresh;
			else 
				table[j][i]=table[j][i]+u*thresh;
		}
	}	

for(i=4;i<=4;i++)
	{
		for(j=0;j<32;j++)
		{
		thresh=(3.6*sigma)
        
			if(myfabs(table[j][i])<thresh)
				table[j][i]=0;
			else if(table[j][i]>=thresh)
				table[j][i]=table[j][i]-u*thresh;
			else 
				table[j][i]=table[j][i]+u*thresh;
		}
	}	



}

float CGyroscope_sDlg::median(float table[32][5])
{
	int i,j;
   float t=0;
   for(i=0;i<32;i++)
   {
   		temp[i]=myfabs(table[i][1]);
   }
   
   for(j=1;j<32;j++)
   {
	   for(i=0;i<32-j;i++)
	   {
		   if(temp[i]>temp[i+1])
		   {
			   t=temp[i];temp[i]=temp[i+1];temp[i+1]=t;
		   }
	   }
   }
   t=(temp[32/2-1]+temp[32/2])/2;
   
   return t;
}


float CGyroscope_sDlg::flp(float output[])
{
	int i,j;
float e,sum,thresh1,b;
float B[20],data_forecast[20];
double phi;

b = 1.0/20;
for(i=0 ; i<20 ; i++)  //初始权重
   {
    B[i] = b;
    }

phi = pow(10,-8);

for(i=0 ; i<32-20 ; i++)    //前向预测
   {
   sum = 0;
   for(j=0 ; j<20 ; j++)
      {
       data_forecast[j] = output[i+j]; 
       sum = sum + (B[j] * data_forecast[j]);
       }
   
   e=fabs((output[i+20])-sum);
   if (e<0.1*output[20])
      {
       phi=0.99*phi;
       }
      
   thresh1 = phi*e;       
   for(j=0;j<20;j++)               //更新权重
      {
       B[j] = B[j] + thresh1*data_forecast[j];
       }
     
     output[i]=sum;                //滤波后的输出信号
    }
    return output[31];

}