互补滤波及仿真记录

互补滤波及仿真记录

http://www.znczz.com/thread-84046-1-1.html

 

 

【原创】姿态估计http://www.ourdev.cn/bbs/bbs_content.jsp?bbs_sn=4769138&bbs_page_no=1&search_mode=3&search_text=lijieamd&bbs_id=1025

 

http://www.ourdev.cn/bbs/bbs_content.jsp?bbs_sn=4831447

 

http://zhidao.baidu.com/question/381117050.html

 

互补滤波器资料http://www.znczz.com/thread-84046-1-1.html

 

大神们用的互补滤波的两个权值是动态调整的么？

http://www.znczz.com/forum.php?mod=viewthread&tid=81234

 

大家帮忙看一下互补波形吧，站不起来，对自己波形没信心了

http://www.znczz.com/thread-84775-3-1.html

 

 

 

 

//-------------------------------------------------------
//互补滤波
//-------------------------------------------------------
static float angle,angle_dot;                 //外部需要引用的变量
//-------------------------------------------------------
static float bias_cf;
static const float dt=0.01;
//-------------------------------------------------------
void complement_filter(float angle_m_cf,float gyro_m_cf)
{
bias_cf*=0.0001;                        //陀螺仪零飘低通滤波；500次均值；0.998
bias_cf+=gyro_m_cf*0.009;                   //0.002
angle_dot=gyro_m_cf-bias_cf;                   
angle=(angle+angle_dot*dt)*0.95+angle_m_cf*0.04;        
//加速度低通滤波；20次均值；按100次每秒计算，低通5Hz；0.90 0.05
}      

 

 

这个公式其实就是低通滤波器（也叫惯性滤波）的离散公式：
Y(k) = (Tc/(Tc+Ts))*Y(k-1) + (Ts/(Tc+Ts))*X(k)
其中Tc为滤波时间系数，Ts为采样时间，X(k)为当前采样值,Y(k)为滤波器输出值,Y(k－1)为滤波器的上次输出。
Tc = 1/wc = 1/(2*pi*fc);fc也就是通常所说的截止频率。如果知道信号的频率要求，fc应该就可以确定下来。

 

 

互补滤波matlab仿真    

%互补滤波仿真系统

%angle=(angle+angle_dot*dt)*0.910+angle_m_cf*0.090;

clc;

clear;

dt=0.01;

j=0;

angle=0;

for i=0:0.01:50   

x_acc = sin(i)+4*(-1+(1-(-1))*rand(1));

angle_dot =cos(i)+ 4*(-1+(1-(-1))*rand(1));%cos(i)+

angle= 0.98 * (angle + angle_dot * dt) + 0.02 * x_acc;

plot (i,x_acc,'r',i,angle_dot,'b',i,angle,'g');

hold on

end

 

 

上图陀螺仪给的值为余弦函数+ 4*[-1 1]之间的随机数

加速度的值为正弦函数+4*[-1 1 ]之间的随机数

 

 

离散化的一阶低通滤波器设计

 

将普通硬件RC低通滤波器的微分方程用差分方程来表求，变可以采用软件算法来模拟硬件滤波的功能，经推导，低通滤波算法如下：
Yn=a* Xn+（1-a）*Yn-1
式中 Xn——本次采样值
Yn-1——上次的滤波输出值；
，a——滤波系数，其值通常远小于1；
Yn——本次滤波的输出值。
由上式可以看出，本次滤波的输出值主要取决于上次滤波的输出值 （注意不是上次的采样值，这和加权平均滤波是有本质区别的），本次采样值对滤波输出的贡献是比较小的，但多少有些修正作用，这种算法便模拟了具体有教大惯性的低通滤波器功能。滤波算法的截止频率可用以下式计算：
          fL= a/（2*Pi*t）   pi为圆周率3.14…
式中 a——滤波系数；
，t——采样间隔时间；
例如：当t=0.5s（即每秒2次），a=1/32时；
fL=（1/32）/（2*3.14*0.5）=0.01Hz
当目标参数为变化很慢的物理量时，这是很有效的。另外一方面，它不能滤除高于1/2采样频率的干搅信号，本例中采样频率为2Hz，故对1Hz以上的干搅信号应采用其他方式滤除，
低通滤波算法程序于加权平均滤波相似，但加权系数只有两个：a和1-a。为计算方便，a取一整数，1-a用256-a，来代替，计算结果舍去最低字节即可，因为只有两项，a和1-a，均以立即数的形式编入程序中，不另外设表格。虽然采样值为单元字节（8位A/D）。为保证运算精度，滤波输出值用双字节表示，其中一个字节整数，一字节小数，否则有可能因为每次舍去尾数而使输出不会变化。

 

 

低通滤波器的截止频率是什么意思

9.5KHz～1.5KHz的方波输入低通滤波器，截止频率是多少啊

 

无论是什么样的滤波器，一般都是指-3db的位置，也就是说从滤波器的通带的增益算起，下降-3db的位置。由于db的计算公式是20*log10(x),x为信号某一个频率上真正的幅值，所以稍加计算可得，-3db实际相当于频率增益下降到了原来的0.707，而不是一半。

对于低通滤波器，是低于某个频率为通带，高于某个频率为阻带。如果要输入9.5KHz～1.5KHz的方波，则9.5KHz～1.5KHz这些频率必须在通带之内，所以滤波器的截止频率要高于9.5KHz。

 

 

This defines where the boundary between trusting the gyroscope and trusting the accelerometer

is. For time periods shorter than half a second, the gyroscope integration takes precedence and

the noisy horizontal accelerations are filtered out. For time periods longer than half a second,

the accelerometer average is given more weighting than the gyroscope, which may have drifted

by this point.

 

在调试的过程中我们需要看波形，我们应该看什么波形

一个陀螺仪的  一个加速度的  一个融合之后的  还有一个陀螺仪这次算出的值

 

 

简易互补滤波器可表示为
angle,-口胛。(angle。．1+gyro．。dt)+aacc。‘‰ (2·7)
式中agyr。I上z+dt为陀螺仪的高通滤波系数，口。锹一老为加速度计的低通滤
波系数。T为滤波器的时间常数。对于低通滤波而言，变化周期大于时间常数的信
号可以完整的通过；变化周期小于时间常数的信号被过滤掉；高通滤波刚好相反。
邀个时间常数，是更相信陀螺仪还是更相信加速度计的一个界限值。变化周期比
百短的，陀螺仪积分的角度更让人相信，而加速度计噪声被过滤掉；变化周期比百
长的，加速度计测定的角度值比陀螺仪更重要，因为此时陀螺仪会发生漂移了。
针对陀螺仪的高通滤波和针对加速度计的低通滤波两部分恰好可组成一个互
补滤波器。可以看到，两个滤波系数相加结果为1，所以滤波结果是一个比较精确
的、线性的角度估计值。
在大多情况下，滤波器通常是按照这样的顺序设计的汹1。首先选取时间常数
t，然后用t来计算滤波系数。比如，如果陀螺仪平均每秒漂移10，那就可以选取
时间常数t--Is，这样就可以保证每秒的漂移量不会超过1。。但该常数值越小，通
过的加速度计噪声就越多。所以必须找到一个最为合适的中间值，找到该值最好
的办法是实验测试。采样频率也是一个重要的数值。例如，程序的更新时间(循
环一次所用时间)为25ms，如果选定时间常数为O．8s，则滤波系数应该这样计算：
口胛一而鬲0．正8函一0．97， 口一一1一口辨一0．03
所以滤波器为：
angle一0．97·(angle+gyro·0．025)+0．03·吃础 (2．8)
通过表2．2可以看出增加了陀螺仪高通滤波的效果。假设系统以90。／s的角速
8单轴双轮自平衡代步车的研究与设i-l-J．C．ZAang 2009
滤波角度(度)O．00 2．48 4．89 7．Z3 9．50 9．51 9．53 9．54 9．55 9．57 9．58
程序循环一次用时25ms，计算可知，前四次采样时系统角速度为900／s，此时
针对陀螺仪的高通滤波起主要作用，可以看出角度评估值从O快速增加到9．500；
程序运行至第五次时，系统已转至100保持不变，从此时开始后六次采样角速度值
都是O，这时仅有针对加速度计的低通滤波起作用，角度值缓慢增加。
这样，利用加速度计输出来消除陀螺仪的漂移，使最终得到倾角估计更为精
确。这就是传感器数据融合中典型的状态估计问题。状态估计的目的对目标过去
的状态进行平滑、对现在的状态进行滤波和对未来的状态进行预测。