对一阶二阶低通数字滤波器的理解(基于匿名微型六轴)



* 文件名 :ANO_Filter.cpp

* 描述 :滤波器相关函数。结论:一般阶次越高,传递函数越复杂。 
**********************************************************************************/
include “ANO_Filter.h”/*
IIR滤波器是无限冲击响应滤波器,
其优点:
1. 采用模拟原型滤波的标准设计,容易理解。
2. 可以用低阶设计实现,并且可以高速运行
3. 对于相同公差设计方案,其阶数比FIR短。
4. 可以采用闭环设计
其缺点:
1. 非线性相位
2. 可能会出现极限环
3. 多频道设计困难,只能设计低通、高通和带通
4. 反馈会引入不稳定
5. 非常难得到高速流水线设计 
*/

/———————-一阶低通滤波器系数计算————————-/
float ANO_Filter:: LPF_1st_Factor_Cal(float deltaT, float Fcut)

//deltaT:时间差 F-CUT:超高速切割,低通滤波器截止频率,单位:Hz

{
return deltaT / (deltaT + 1 / (2 * M_PI * Fcut));
}

/*
想要达到更好的滤波效果,FIR或者IIR滤波器是更好的选择。
笔者测试2阶30Hz的Butterworth滤波器虽然在平滑性RMSE只比
窗口平均滑动差了一点(但是比LPF要好),但是数据实时性
性能指标上比前者响应速度提高了近一倍
。因此在制作四轴的
进阶阶段,可以考虑将窗口平均滑动换成Butterworth滤波器。
*/ 

/———————-一阶低通滤波器————————/
//lpf_factor:一阶滤波系数 oldData:ACC的上一次的数据 newData:ACC的最新数据

//算法基础:Yn = a * Xn + (1-a) * Yn-1 
//上式解释:a:滤波系数,其值通常远小于1 ;Xn:本次采样值: Yn-1;上次的滤波输出值;Yn:本次滤波的输出值。
/**********************************************************************************
由上式可以看出,本次滤波的输出值主要取决于上次滤波的输出值
(注意不是上次的采样值,这和加权平均滤波是有本质区别的),
本次采样值对滤波输出的贡献是比较小的,但多少有些修正作用,
这种算法便模拟了具体有教大惯性的低通滤波器功能。
功效:当目标参数为变化很慢的物理量时,这是很有效的。
**********************************************************************************/
Vector3f ANO_Filter:: LPF_1st(Vector3f oldData, Vector3f newData, float lpf_factor)
{
return oldData * (1 – lpf_factor) + newData * lpf_factor;
}

/———————-二阶低通滤波器系数计算————————-/
void ANO_Filter:: LPF_2nd_Factor_Cal(LPF2ndData_t* lpf_data)
{
//截止频率(中心频率f0):30Hz 采样频率fs:500Hz
lpf_data->b0 = 0.1883633f;
lpf_data->a1 = 1.023694f;
lpf_data->a2 = 0.2120577f;
}
/*

输出公式:Y(n)= b0xn + b1xn-1 + b2xn-2 – (a1xn-1 + a2*xn-2) 
式中a1,a2,b0,b1,b2是二阶滤波器IIR系数,其决定滤波器的频响应曲线以及增益。
如何求a0,a1,a2,b0,b1,b2
对于一个二阶IIR滤波器,标准的技术指标如下:
1. 中心频率f0;
2. 采样频率fs;
3. 增益db;
4. 品质因数;

中心频率: 通常定义为带通滤波器(或带阻滤波器)的两个3 dB点之间的中点,一般用两个3 dB点的算术平均来表示 。其实低通和高通滤波器也有中心频率,只不过它的定义和带通就不一样了,它就等于我们通常说的截止频率.但我们在说低通高通时,都是用截止频率,而几乎不用其中心频率。不过在做归一化时就会有这个概念了。那时可以看到,低通高通的归一化,截止频率=截止频率/中心频率=1.

通常,对一个滤波器的要求,我们主要给出以下技术规格:中心频率frequency,采样频率sampleRate,增益dBgain,品质因数Q。

根据上面的技术指标,可以确定以下几个通用计算量:
A = sqrt[ 10^(dBgain/20) ]
omega = 2pi*frequency/sampleRate //传说中的角频率

sin = sin(omega)

cos = cos(omega)
alpha = sin/(2*Q)

所以二阶IIR高通滤波器系数的计算:
b0=(1+cos)/2;
b1=-(1+cos);
b2=(1+cos)/2;
a0=1+alpha;
a2=1-alpha;
二阶IIR低通滤波器系数的计算:
b0=(1-cos)/2;
b1=1-cos;
b2=(1-cos)/2;
a0=1+alpha;
a1=-2cos; a2=1-alpha;
二阶IIR带通滤波器的系数的计算:
b0=sin/2=Q
alhpa;
b1=0;
b2=-sin/2=-Qalpha; a0=1+alpha;
a1=-2
cos;
a2=1-alpha; 
/ /
众所周知, 加速度计的高频噪声较严重,尤其是廉价的加速度传感器,针对加速度滤波,
目前所能想到的滤波方法无非是滑动平均滤波(低通滤波),至于效果能否满足需求,
现在还不得而知。
/ /———————-二阶低通滤波器————————/ Vector3f ANO_Filter:《对一阶二阶低通数字滤波器的理解(基于匿名微型六轴)》PF_2nd(LPF2ndData_t lpf_2nd, Vector3f newData)
{
Vector3f lpf_2nd_data;

//对于二阶IIR滤波器,输出公式:Y(n)= b0xn + b1xn-1 + b2xn-2 – (a1xn-1 + a2*xn-2)

//此处忽略了b1 * xn-1 + b2 * xn-2 ,b1和b2的系数为0 
lpf_2nd_data = (newData * lpf_2nd->b0)+ (lpf_2nd->lastout * lpf_2nd->a1) – (lpf_2nd->preout * lpf_2nd->a2);

//更新数值,留待下次运算 
lpf_2nd->preout = lpf_2nd->lastout;
lpf_2nd->lastout = lpf_2nd_data;

return lpf_2nd_data;
}

/———————-互补滤波器系数计算————————-/
float ANO_Filter::CF_Factor_Cal(float deltaT, float tau)
{
return tau / (deltaT + tau);
}

/———————-一阶互补滤波器—————————–/
/***********************************************************************************
科普:互补滤波
对mpu6050来说,加速度计对四轴或小车的加速度比较敏感,取瞬时值计算倾角误差比较大;
而陀螺仪积分得到的角度不受小车加速度的影响,但是随着时间的增加积分漂移和温度漂移带来
的误差比较大。所以这两个传感器正好可以弥补相互的缺点。不过要怎么弥补呢?经过上面的介绍
是否感觉到可以用滤波器做文章呢? 
这里讲的互补滤波就是在短时间内采用陀螺仪得到的角度做为最优,定时对加速度采样来的角度
进行取平均值来校正陀螺仪的得到的角度。就是,短时间内用陀螺仪比较准确,以它为主;长时间用
加速度计比较准确,这时候加大它的比重,这就是互补了
,不过滤波在哪里加速度计要滤掉高频信号,
陀螺仪要滤掉低频信号,互补滤波器就是根据传感器特性不同,通过不同的滤波器(高通或低通,互补的),
然后再相加得到整个频带的信号,例如,加速度计测倾角,其动态响应较慢,在高频时信号不可用,
所以可通过低通抑制高频;陀螺响应快,积分后可测倾角,不过由于零漂等,在低频段信号不好。
通过高通滤波可抑制低频噪声。将两者结合,就将陀螺和加表的优点融合起来,得到在高频和低频都较好
的信号,互补滤波需要选择切换的频率点,即高通和低通的频率。 
**********************************************************************************/
/***********************************************************************************
由于陀螺零点漂移和离散采样产生的累积误差, 由陀螺得到的四元数只能保证短期的精度, 需要
使用加速度计和磁力计对其进行矫正
由于四旋翼飞行器所使用的加速度计具有长期可信、短期噪声较大的特点;陀螺仪
具有短期可信,长期不稳定的特点,因此可以使用互补滤波对其进行融合。简而言之,
互补滤波相当于将低通滤波器与高通滤波器结合在一起,对加速度计进行低通滤波,而
对陀螺仪进行高通滤波,通过整定合适的参数来得到一个较好的滤波器特性

**********************************************************************************/
Vector3f ANO_Filter::CF_1st(Vector3f gyroData, Vector3f accData, float cf_factor)

return (gyroData * cf_factor + accData *(1 – cf_factor));

}

    原文作者:feiyingzaishi
    原文地址: https://blog.csdn.net/feiyingzaishi/article/details/77898231
    本文转自网络文章,转载此文章仅为分享知识,如有侵权,请联系博主进行删除。
点赞