C语言实现——hampel滤波函数

hampel的作用一般为去除波形曲线毛刺
MATLAB中有关hampel的函数有两个，第一个为hampel.m，改写C代码如下:

void hampel20(float Y1[], float uhy[])
{
	int len = 512;
	float Y1_beg[2 * N];
	float Y1_end[2 * N];
	memmove(Y1_beg, Y1, sizeof(float) * 2 * N);
	//for (int i = 0; i < 2 * N; i++)
	//{
	//	Y1_beg[i] = Y1[i];
	//}
	//头尾hampel
	float m_beg[N], m_end[N], mm_beg[N], mm_end[N];
	for (int i = 0; i < N; i++)
	{
		qsort(Y1_beg, i + N + 1, sizeof(Y1_beg[0]), cmp_ascending);
		if ((i + 1 + N) % 2 == 0)//偶数
		{
			m_beg[i] = (Y1_beg[(i + 1 + N) / 2 - 1] + Y1_beg[(i + 1 + N) / 2]) / 2;
		}
		else
		{
			m_beg[i] = (Y1_beg[(i + N) / 2]);
		}
	}
	for (int i = 0; i < N; i++)
	{
		int temp_Y1_end = 0;
		for (int j = 512 - 2 * N + i; j < 512; j++)
		{
			Y1_end[temp_Y1_end] = Y1[j];
			temp_Y1_end += 1;
		}
		qsort(Y1_end, 2 * N - i, sizeof(Y1_end[0]), cmp_ascending);
		if ((2 * N - i) % 2 == 0)//偶数
		{
			m_end[i] = (Y1_end[(2 * N - i) / 2] + Y1_end[(2 * N - i) / 2 - 1]) / 2;
		}
		else
		{
			m_end[i] = (Y1_end[(2 * N - i) / 2]);
		}
	}
	float Ym_beg[2 * N], Ym_end[2 * N];
	for (int i = 0; i < N; i++)
	{
		for (int j = 0; j < i + N +1; j++)
		{
			Ym_beg[j] = fabs(Y1[j] - m_beg[i]);
			/*Ym_end[j] = fabs(Y1[512-2*N+j] - m_end[i]);*/
		}
		qsort(Ym_beg, i + N + 1, sizeof(Ym_beg[0]), cmp_ascending);
		if ((i + 1 + N) % 2 == 0)//偶数
		{
			mm_beg[i] = (Ym_beg[(i + 1 + N) / 2 - 1] + Ym_beg[(i + 1 + N) / 2]) / 2;
		}
		else
		{
			mm_beg[i] = (Ym_beg[(i + N) / 2]);
		}
		int temp_Ym_end = 0;
		for (int j = 512 - 2 * N + i; j < 512; j++)
		{
			Ym_end[temp_Ym_end] = fabs(Y1[j] - m_end[i]);
			temp_Ym_end += 1;
		}
		qsort(Ym_end, 2 * N - i, sizeof(Ym_end[0]), cmp_ascending);
		if ((2 * N - i) % 2 == 0)//偶数
		{
			mm_end[i] = (Ym_end[(2 * N - i) / 2] + Ym_end[(2 * N - i) / 2 - 1]) / 2;
		}
		else
		{
			mm_end[i] = (Ym_end[(2 * N - i) / 2]);
		}
	}
	for (int i = 0; i < N; i++)
	{
		if (fabs(Y1[i] - m_beg[i]) > 3 * 1.482602218505602*mm_beg[i])
		{
			uhy[i] = m_beg[i];
		}
		else
		{
			uhy[i] = Y1[i];
		}
		if (fabs(Y1[512 - N + i] - m_end[i]) > 3 * 1.482602218505602*mm_end[i])
		{
			uhy[512 - N + i] = m_end[i];
		}
		else
		{
			uhy[512 - N + i] = Y1[512 - N + i];
		}
	}
	//中间hampel

	for (int i = N; i < 512 - N; i += 1)
	{
		float window_value[2 * N + 1] = { 0 };
		float wwindow_value[2 * N + 1] = { 0 };
		for (int j = 0; j < 2 * N + 1; j++)
		{
			window_value[j] = Y1[i - N + j];
		}
		qsort(window_value, 2 * N + 1, sizeof(window_value[0]), cmp_ascending);
		float mid_value = window_value[N];
		for (int j = 0; j < 2 * N + 1; j++)
		{
			wwindow_value[j] = fabs(window_value[j] - mid_value);
		}
		qsort(wwindow_value, 2 * N + 1, sizeof(wwindow_value[0]), cmp_ascending);
		float mmid_value = wwindow_value[N];
		if (fabs(Y1[i] - mid_value) > 3 * 1.482602218505602* mmid_value)
		{
			uhy[i] = mid_value;
		}
		else
		{
			uhy[i] = Y1[i];
		}
	}

}

还有一个dsp类中的HampelFilter方法，原理上dsp.HampelFilter和hampel.m是相同的，但是dsp.HampelFilter左侧使用了0补边，右侧选择遗弃，相当于仅对中间滤波。当波形长度>>窗长时两个函数相似，否侧hampel.m效果更好。

void dsp_hampel(float Y1[],float uhy[])
{
	int  temp = 0;
	for (int i = 0; i < 512 - n; i += 1)
	{
		float window_value[2 * n + 1] = { 0 }, wwindow_value[2 * n + 1] = { 0 };
		if (temp < n)
		{
			uhy[temp] = 0;//前面一部分为0
			temp += 1;
			i -= 1;
			continue;
		}
		int j = 0;
		if (i < n - 1)
		{
			for (j = 0; j < n - i; j++)
				window_value[j] = 0;
		}
		int temp_i = 0;
		while (j < 2 * n + 1)
		{
			window_value[j] = Y1[temp_i + i];
			j++;
			temp_i++;
		}
		qsort(window_value, 2 * n + 1, sizeof(window_value[0]), cmp_ascending);
		float mid_value = window_value[n];
		for (int j = 0; j < 2 * n + 1; j++)
		{
			wwindow_value[j] = fabs(window_value[j] - mid_value);
		}
		qsort(wwindow_value, 2 * n + 1, sizeof(wwindow_value[0]), cmp_ascending);
		float mmid_value = wwindow_value[n];
		if (fabs(Y1[i] - mid_value) > 3 * 1.482602218505602* mmid_value)
		{
			//if (i > (512 - ((2 * n) / 2) - 1)|i<30)
			//	continue;
			uhy[i + n] = mid_value;
		}
		else
		{
			//if (i > (512 - ((2 * n) / 2) - 1)|i<30)
			//	continue;
			uhy[i + n] = Y1[i];
		}
	}
}

调用函数为：

#include <stdlib.h>
#include "math.h"
#include "stdio.h"
#define n 30//hampel的窗一侧的长度，本例总长度为61
int cmp_ascending(const void *_a, const void *_b)
{
	float* a = (float*)_a; //强制类型转换
	float* b = (float*)_b;
	return *a > *b ? 1 : -1; //特别注意
}
void hampel(float Y1[], float uhy[]);
void dsp_hampel(float Y1[], float uhy[]);
int main() {
float uhy[512]={0};//滤波后的数组
float Y1[512]={.......}//原始数组
	dsp_hampel(Y1,uhy);
	hampel(Y1,uhy);
	return 0；
}