电机控制（二）--- 滤波算法

一、限幅滤波算法

        （1）确定两次采样允许的最大偏差值（设为A）

                每次检测到新的采样值时判断：

                如果本次值与上次值之差<=A，则本次采样值有效；

                如果本次值与上次值之差 > A，则本次采样值无效。

        （2）如果本次值无效，对于本次采样可以做不同的处理：

                比如：

                放弃本次采样值，使用上次的采样值；

                本次采样值=上次采样值±限幅A；

                放弃本次采样，重新采样。

        （3）优点：能够消除因偶然波动引起的误差。

        （4）缺点：无法抑制周期性的干扰，平滑性差。

        （5）示例代码：

	#define variation_Range 10    //定义的误差值
	char Bounds_Filter(char new_value,char old value)
	{
		if(abs(new_value-old_value)>variation_Rang){    //两个数据的误差值
			reture old_value;
		}
		return new_value;
	}

---

二、中位值滤波算法

        （1）连续采样奇数个数据，然后对数据进行排序，从小到大，取中间值作为本次采样值；

        （2）优点：可以消除偶然因素引起的脉冲干扰，消除毛刺，使数据更加平滑，适用于变化缓慢的较为线性的采样系统；

        （3）缺点：采样值快速变化的系统不宜使用

        （4）示例代码：

	//采样次数samples_Num次，然后进行冒泡排序，最后取中间值
	#define  samples_Num  5 //采样次数
	int median_Filter()
	{
		unsigned int value[samples_Num] = {0};
		unsigned int count, i, j, temp;
		for(count = 0; count < samples_Num; count++){    //采样数据
			vlaue[count] = Read_ADC();
		} 
		for(j = 0; j<samples_Num; j++){                  //冒泡排序
			for(i = 0; i<samples_Num;i++){
				if(value[i]>value[i+1]){
					temp = value[i];
					value[i] = value[i+1];
					value[i+1] = temp;
				}
			}
		}
		return value[(samples_Num-1)/2]                  //返回中位值
	}

---

三、一阶低通滤波算法

        （1）滤波结果=a*本次采样值+（1-a）*上次滤波结果；

        （2）a取值为0~1之间就是将新的采样值与上次的滤波结果计算一个加权平均值；

        （3）a的取值决定了算法的灵敏度，a越大，新采集的值占的权重越大，算法越灵敏，但平顺性差；相反，a越小，新采集的值占的权重越小，灵敏度差，但平顺性好；

        （4）优点：对周期性干扰具有良好的抑制作用适用于波动频率较高的场合；

        （5）缺点：相位滞后,灵敏度低；

        （6）示例代码：

	float final = 0.0F;
	float a = 0.25F;    //a的取值为0~1
	float first_order_filter(float data)
	{
		final = a*data + (1-a)*final;    //两次数据乘上各自的权重
		return  (final);
    }

---

四、算术平均滤波算法

        （1）在某一时刻对信号进行多次采样，对采样值进行算术平均（平均值），作为该时刻的信号采样值，连续采样次数具体情况而定；

        （2）优点：对抑制周期性干扰和随机干扰有良好的效果；

        （3）缺点：采样比较耗时，并且可能随某个数据干扰而产生较大的浮动；

        （4）示例代码：

	#definen  N  10
	int value = 0;
	int Arithmetic_Filter()
	{
		for(int i = 0; i < N; i++){
			value += Read_ADC();
		}
		return (value/N);
    }

---

五、中位值平均滤波算法

        （1）连续采样N个数据，去掉最大值，最小值，然后计算N-2个数据的平均值，作为采样值；

        （2）优点：对于偶然性的脉冲信号干扰，可消除由于脉冲信号所引起的采样值偏差

        （3）缺点：用时比较久

        （4）示例代码：

	#define  N  12
	unsigned int Med_Avg_Filter())
	{
		unsigned int count, i, j, temp;
		unsigend int value_buff[N];
		int ad_sum = 0;
		//采样N次
		for(count = 0; count < N; count++ ){
			value_buff[count] = Read_ADC();
			
		}
		//冒泡排序升序，舍去最大值，最小值
		for(j = 0; j < N-1; j++){
			for(i = 0; i < N-j-1; i++){
				if(value[i] > value[i+1]){
					temp = value_buff[i];
					value_buff[i] = value_buff[i+1];
					value_buff[i+1]=temp;
				}
			}
		}
		//求取中间项和
		for( count = 1; count < N-1; count++ ){
			ad_sum += value_buff[count];
		}
		return (ad_sum/(N-2));
	}

---

六、滑动算术平均值滤波算法

        （1）滑动平均值滤波是指先在RAM中建立一个数据缓冲区，依顺序存放N个采样数据，每次采样到一个新数据放入队尾，并扔掉原来队首的一次数据，遵循先进出，而后求包括新数据在内把队列中的N个数据进行算术平均运算,就可获得新的滤波结果；

        （2）优点：对周期性干扰有良好的抑制作用，平滑性好，适用于高频系统；

        （3）缺点：灵敏度低，对干扰抑制作用低，不适于脉冲干扰较严重的场合，对RAM的开销大；

        （5）优化：可以每次减去的不是队首的值，而是上一次滤波的平均值；

        （4）示例代码：

//平滑均值滤波
//    |采集对象     N
//    |流量        12            
//    |压力        4
//    |液面        4~12
//    |温度        1~4
//     GetValue()    采集数据函数
#define N 10
int value_buf[N];
int sum=0;
int curNum=0;
int moveAverageFilter()
{
    if(curNum < N){
        value_buf[curNum] = GetValue();
        sum += value_buf[curNum];
			  curNum++;
        return sum/curNum;
    }
    else{
        sum -= sum/N;
        sum += GetValue();
        return sum/N;
    }
}

---

七、卡尔曼滤波算法

        （1）根据当前的仪器"测量值" 和上一刻的 “预测量” 和 “误差”，计算得到当前的最优量，再预测下一刻的量；

        （2）把误差纳入计算，而且分为预测误差和测量误差两种，通称为噪声；

        （3）误差独立存在，始终不受测量数据的影响；

        （4）优点：巧妙的融合了观测数据与估计数据，对误差进行闭环管理，将误差限定在一定范围。适用性范围很广，时效性和效果都很优秀；

        （5）缺点：需要调参，参数的大小对滤波的效果影响较大，工作较为繁琐；

        （6）示例代码：

//卡尔曼滤波
int KalmanFilter(int inData)
{
		static float prevData = 0;   //先前数值

		static float p = 10.0F;         //q控制误差 
        static float q = 0.001F;        //测量方差
        static float r = 0.001F;        //r控制响应速度
        static float kGain = 0.0F;      //卡尔曼增益  
	
		p = p + q;
		kGain = p / ( p + r );                                     //计算卡尔曼增益
		inData = prevData + ( kGain * ( inData - prevData ) );     //计算本次滤波估计值
		p *= ( 1 - kGain ) ;                                       //更新测量方差
		prevData = inData;
		return inData;                                             //返回滤波值
}