嵌入式常用滤波算法

先贴出来，后续优化

#include "filtering.h"

//一阶互补滤波
int firstOrderFilter(int newValue, int oldValue, float a)
{
	return a * newValue + (1-a) * oldValue;
}

/* 滑动平均滤波 */
int slip_ave_filter(slip_ave_filter_def *p_adc)
{
	if(p_adc == NULL)
	{
		return -1;
	}
	
	if(p_adc->filter_num == 0)
	{
		return p_adc->input_value;
	}
	
    if(p_adc->curNum < p_adc->filter_num)
    {
        p_adc->sum += p_adc->input_value;
		p_adc->curNum++;
        return p_adc->sum/p_adc->curNum;
    }
    else
    {
        p_adc->sum -= p_adc->sum/p_adc->filter_num;
        p_adc->sum += p_adc->input_value;
        return p_adc->sum/p_adc->filter_num;
    }
}



/* 中值滤波算法 */
int middleValueFilter(median_filter_def* p_adc)
{
	if(p_adc == NULL)
	{
		return -1;
	}
	
	/* 效率问题，后续优化 */
	p_adc->value_buf[p_adc->i % MD_FILTER_NUM] = p_adc->input_value;	
	(p_adc->i)++;
	
	if(p_adc->i < MD_FILTER_NUM)
	{
		return p_adc->value_buf[p_adc->i];
	}
	
    for(p_adc->j = 0 ; p_adc->j < MD_FILTER_NUM - 1; ++(p_adc->j))
    {
        for(p_adc->k = 0; p_adc->k < MD_FILTER_NUM - (p_adc->j) - 1; ++(p_adc->k))
        {
            //从小到大排序，冒泡算法
            if(p_adc->value_buf[p_adc->k] > p_adc->value_buf[(p_adc->k)+1])
            {
                p_adc->temp = p_adc->value_buf[p_adc->k];
                p_adc->value_buf[p_adc->k] = p_adc->value_buf[(p_adc->k) + 1];
                p_adc->value_buf[(p_adc->k) + 1] = p_adc->temp;
            }
        }
    }
	
    return p_adc->value_buf[(MD_FILTER_NUM-1)/2];
}

//算术均值滤波算法
int averageFilter(int N)
{
   int sum = 0;
   short i;
   for(i = 0; i < N; ++i)
   {
        sum += HAL_ADC_GetValue(&hadc1);	
   }
   return sum/N;
}

//平滑均值滤波
#define N 10
int value_buf[N];
int sum=0;
int curNum=0;
int moveAverageFilter()
{
    if(curNum < N)
    {
        value_buf[curNum] = HAL_ADC_GetValue(&hadc1);
        sum += value_buf[curNum];
			  curNum++;
        return sum/curNum;
    }
    else
    {
        sum -= sum/N;
        sum += HAL_ADC_GetValue(&hadc1);
        return sum/N;
    }
}

//限幅平均滤波
#define A 50
#define M 12
int data[M];
int First_flag=0;
int LAverageFilter()
{
  int i;
  int temp,sum,flag=0;
  data[0]=HAL_ADC_GetValue(&hadc1);
  for(i=1;i<M;i++)
	{
		temp=HAL_ADC_GetValue(&hadc1);
		if((temp-data[i-1])>A||((data[i-1]-temp)>A))
		{
		  i--;flag++;
		}
		else
		{
			data[i]=temp;
		}
	}
  for(i=0;i<M;i++)
  {
    sum+=data[i];
  } 
  return  sum/M;
}

/* 卡尔曼滤波 */
int KalmanFilter(int inData)
{
	static float prevData = 0;                                 //先前数值
	static float p = 10, q = 0.001, r = 0.001, kGain = 0;      // q控制误差  r控制响应速度 

	p = p + q;
	kGain = p / ( p + r );                                     //计算卡尔曼增益
	inData = prevData + ( kGain * ( inData - prevData ) );     //计算本次滤波估计值
	p = ( 1 - kGain ) * p;                                     //更新测量方差
	prevData = inData;
	return inData;                                             //返回滤波值
}

#ifndef __FILTERING_H
#define __FILTERING_H
#include "stdint.h"
#include "stdio.h"
/* 滑动平均滤波 */
typedef struct 
{
	int sum;			//不需要定义
	int curNum;			//不需要定义
	int input_value;	//输入值
	int filter_num;		//滤波次数
}slip_ave_filter_def;
int slip_ave_filter(slip_ave_filter_def *p_adc);


int firstOrderFilter(int newValue, int oldValue, float a);


#define MD_FILTER_NUM 	15

typedef struct 
{
	uint32_t i,j,k,temp;			//不需要定义
	int input_value;	//输入值
	int value_buf[MD_FILTER_NUM];	//fifo
}median_filter_def;
int middleValueFilter(median_filter_def* p_adc);

int averageFilter(int N);
int moveAverageFilter();
int LAverageFilter();
int KalmanFilter(int inData);

#endif