滤波算法---一点点理解

1、一阶滤波

（1）、介绍

        一阶滤波算法是比较常用的滤波算法，它的滤波结果=a*本次采样值+（1-a）*上次滤波结果，其中，a为0~1之间的数。一阶滤波相当于是将新的采样值与上次的滤波结果计算一个加权平均值。a的取值决定了算法的灵敏度，a越大，新采集的值占的权重越大，算法越灵敏，但平顺性差；相反，a越小，新采集的值占的权重越小，灵敏度差，但平顺性好。

优点：对周期干扰有良好的抑制作用，适用于波动频率比较高的场合，它不用记录历史数据。

缺点：滞后、灵敏度低。

（2）、实现代码

float final=0;
float a=0.25;
 
float firstOrderFilter(float in_data)
{
	final = a*in_data + (1-a)*final;	
	return(final);
}

（3）、示例

下面我们通过一个示例来体会一阶滤波的作用，滤波对象为车速信号，滤波效果如下图所示。图中，横轴为时间，单位：秒，纵轴为速度，单位km/h。其中，蓝色为滤波前的数据，红色为滤波后的数据。有图中可以看出，滤波算法使原始信号变得平滑，但是带来了滞后。

2、卡尔曼滤波

 

二、实现代码

        下面我们通过c++代码来实现卡尔曼滤波算法，所实现的算法为一维滤波算法。首先定义卡尔曼滤波的参数

typedef struct{
    float filterValue;//滤波后的值
    float kalmanGain;//Kalamn增益
    float A;//状态矩阵
    float H;//观测矩阵
    float Q;//状态矩阵的方差
    float R;//观测矩阵的方差
    float P;//预测误差
    float B;
    float u;
}KalmanInfo;

 面是卡尔曼滤波器的初始化函数，在这个函数中，info为卡尔曼滤波参数的指针。初始化的参数是针对一个车速滤波过程的设置。

void Kalm::initKalmanFilter(KalmanInfo *info)
{
    info->A = 1;
    info->H = 1;
    info->P = 0.1;
    info->Q = 0.05;
    info->R = 0.1;
    info->B = 0.1;
    info->u = 0;
    info->filterValue = 0;
}

卡尔曼滤波过程函数，函数的输入info为卡尔曼滤波参数的指针，new_value为新的测量值，函数返回滤波后的估计值。

float Kalm::kalmanFilterFun(KalmanInfo *info, float new_value)
{
    float predictValue = info->A*info->filterValue+info->B*info->u;//计算预测值
    info->P = info->A*info->A*info->P + info->Q;//求协方差
    info->kalmanGain = info->P * info->H /(info->P * info->H * info->H + info->R);//计算卡尔曼增益
    info->filterValue = predictValue + (new_value - predictValue)*info->kalmanGain;//计算输出的值
    info->P = (1 - info->kalmanGain* info->H)*info->P;//更新协方差
    return info->filterValue;
}

三、示例
        下面我们通过是一个车速滤波的示例来体验卡尔曼滤波的效果。通过上面的介绍，R对滤波效果的影响比较大，在这个示例中，我们分别将R取为0.1和0.5，来看一下车速的滤波效果。首先R取为0.1时，滤波效果如下图所示。其中，蓝色线为滤波前的车速，红色线为滤波后的车速。从图中可以看到滤波后的信号与滤波前的信号跟随很好，滞后很小。基本波动被滤掉了，但也带入了一些波动。

下图为R取为0.5时的滤波效果，很明显，这张图信号的跟随效果比上图要差，滞后也多，但是滤波后曲线更平滑。 

带注释的卡尔曼滤波算法

//1. 结构体类型定义
typedef struct 
{
    float LastP;//上次估算协方差 初始化值为0.02		--e(ESTk-1)  上次协方差
    float Now_P;//当前估算协方差 初始化值为0		--预测e(ESTk)	当前估算协方差
    float out;//卡尔曼滤波器输出 初始化值为0
    float Kg;//卡尔曼增益 初始化值为0				--Kk
    float Q;//过程噪声协方差 初始化值为0.001
    float R;//观测噪声协方差 初始化值为0.543		--e(MEAk)  测量误差
}KFP；//Kalman Filter parameter

//2. 以高度为例 定义卡尔曼结构体并初始化参数
KFP KFP_height={0.02,0,0,0,0.001,0.543};

/**
 *卡尔曼滤波器
 *@param KFP *kfp 卡尔曼结构体参数
 *   float input 需要滤波的参数的测量值（即传感器的采集值）
 *@return 滤波后的参数（最优值）
 */
 float kalmanFilter(KFP *kfp,float input)
 {
     //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
     kfp->Now_P = kfp->LastP + kfp->Q;
     
     //卡尔曼增益方程：卡尔曼增益 = k1-1时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
     kfp->Kg = kfp->Now_P  / (kfp->Now_P + kfp->R);
     //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
     kfp->out = kfp->out + kfp->Kg * (input -kfp->out);//因为这一次的预测值就是上一次的输出值
     //更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
     kfp->LastP = (1-kfp->Kg) * kfp->Now_P;
     
     return kfp->out；
 }
/**
 *调用卡尔曼滤波器 实践
 */
int height;
int kalman_height=0;
kalman_height = kalmanFilter(&KFP_height,(float)height);

3、限幅滤波

（1）、对当前值范围进行限幅

对于输入得input判断是否大于最大值，若大于，这使用lastValue替代

float maxFilter(float input,float MAX)
{
   static float lastValue = 0;
   if ( ( input > MAX ) || (input < -MAX ))
	 {
      return lastValue;
	 }
	 lastValue = input;
   return input;
}

（2）、对相邻值之间的偏差进行限幅

#define DELAT_MAX 10
// 定义滤波数据类型
typedef	int filter_type;
filter_type filter(filter_type effective_value, filter_type new_value, filter_type delat_max)
{
	if ( ( new_value - effective_value > delat_max ) || ( effective_value - new_value > delat_max ))
		return effective_value;
	return new_value;
}

4、中值+滑动均值滤波

设定大小为windows的数组Data,sum计算去掉最大最小值的和值，算剩下数的平均值输出

float GildeAverageValueFilter(float NewValue,float *Data,unsigned short int windows)
{
  float max,min;
  float sum;
  unsigned char i;
  Data[0]=NewValue;
  max=Data[0];
  min=Data[0];
  sum=Data[0];
  for(i=windows-1;i!=0;i--)	//循环四次，从后往前  
  {
    if(Data[i]>max) max=Data[i];
    else if(Data[i]<min) min=Data[i];
    sum+=Data[i];
    Data[i]=Data[i-1];	//数据右移
  }
  i=windows-2;
  sum=sum-max-min;
  sum=sum/i;
  return(sum);
}