* 描述 ：滤波器相关函数。结论：一般阶次越高，传递函数越复杂。
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)= b0
xn + b1
xn-1 + b2
xn-2 – (a1
xn-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 = 2
pi
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=-2
cos; a2=1-alpha;
二阶IIR带通滤波器的系数的计算：
b0=sin/2=Q
alhpa;
b1=0;
b2=-sin/2=-Q
alpha; a0=1+alpha;
a1=-2
cos;
a2=1-alpha;
/ /
众所周知， 加速度计的高频噪声较严重，尤其是廉价的加速度传感器，针对加速度滤波，
目前所能想到的滤波方法无非是滑动平均滤波（低通滤波），至于效果能否满足需求，
现在还不得而知。
/ /
———————-二阶低通滤波器————————
/ Vector3f ANO_Filter::LPF_2nd(LPF2ndData_t
lpf_2nd, Vector3f newData)
{
Vector3f lpf_2nd_data;
//对于二阶IIR滤波器，输出公式:Y（n)= b0
xn + b1
xn-1 + b2
xn-2 – (a1
xn-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));
}