滤波函数C语言

.H

#ifndef __FILTER_H
#define __FILTER_H

typedef unsigned char uint8_t;
typedef unsigned short int uint16_t;
typedef unsigned  int uint32_t;

#define MAX_RANGE  10

typedef struct filter{
	float av_x;  //平均步宽
	float data_y[MAX_RANGE]; //原始数据	
	float k_data[MAX_RANGE];  //变化斜率
	float direction;  //平均变化斜率
	float range;  //滤波精度
	float av_y;  //平均值域
	uint16_t w; //保留位数.小数点后几位
}s_filter_t;


float filter_process(float new_x ,float new_y);  //new_x传入固定值,new_y为原始数据,只需要调用此接口即可返回滤波过后的数据



#endif

.C

#include "filter.h"
#include "stdio.h"

//#include "math.h"
 
#define kal_Q  0.0001  /*过程噪声协方差,Q增大，动态响应变快，收敛稳定性变坏*/
#define kal_R  0.011  /*观测噪声协方差,R增大，动态响应变慢，收敛稳定性变好*/



s_filter_t g_filter ;
uint8_t first = 0;

float Kalman(float measure,uint8_t op_flg); //卡尔曼滤波


float Kalman(float measure,uint8_t op_flg)
{
    float x_mid,kg,p_mid;
    static float x_last = 0,p_last = 0;
    float x_now,p_now;
 
    if(op_flg)         /*非初始化*/
    {
        x_mid = x_last;
        p_mid = p_last + kal_Q;
        kg = p_mid / (p_mid + kal_R);
        x_now = x_mid + kg * (measure - x_mid);
        p_now = (1 - kg)*p_mid;
 
        p_last = p_now;
        x_last = x_now;
    }
    else
    {
        x_last = measure;
        p_last = kal_Q;
    }
 
    return x_now;
}


float filter_process(float new_x ,float new_y){
	
	uint8_t i = 0;
	float temp = 0;
	float  rtn = 0;
	//int temp_data = 0;
	
	if(first < MAX_RANGE){
		//初始化
		g_filter.av_x = new_x;
		g_filter.data_y[first] = new_y;
		g_filter.direction = 0;
		g_filter.range = 1;  //增大可以提高变化速度
		g_filter.av_y = 0;
		g_filter.w = 10;
		first ++ ;
		Kalman(new_y, 0); //初始化卡尔曼
		return new_y;
	}
	//削波
	if(new_y > (g_filter.av_y + g_filter.range)){
		new_y = g_filter.av_y + g_filter.range;
	}
	if(new_y < (g_filter.av_y - g_filter.range)){
		new_y = g_filter.av_y - g_filter.range;
	}
	//更新数据
	for(i = 0;i<(MAX_RANGE-1);i++){
		g_filter.data_y[i] = g_filter.data_y[i+1];
	
	}
		g_filter.av_x = new_x; 
		g_filter.data_y[MAX_RANGE-1] = new_y;
	//计算每个点K的值
	for(i = 0; i < (MAX_RANGE - 1) ;i++){
		g_filter.k_data[i] = (g_filter.data_y[i+1] - g_filter.data_y[i])/(g_filter.av_x);
	}
	g_filter.av_y = 0;
	
	for( i = 0;i<MAX_RANGE;i++){
		temp += g_filter.k_data[i];  //平均变化率
		g_filter.av_y += g_filter.data_y[i];
	}
	g_filter.av_y = g_filter.av_y/MAX_RANGE;
	g_filter.direction = temp / MAX_RANGE;
	
	g_filter.av_y = Kalman(g_filter.av_y, 1);  //对移动平均的值进行卡尔曼滤波
	
//	temp_data = (int)g_filter.av_y * g_filter.w;
//
//	g_filter.av_y = temp_data /g_filter.w;
	
	//printf(": %f\n",g_filter.direction);
	
	rtn =  (g_filter.av_x * g_filter.direction) + g_filter.av_y; 
	
	
	//printf("--->: %f\n",rtn);
	return rtn;
}