求一份互补滤波程序

静水深浏

对互补滤波不太了解，求大神指点，谢了

静默深眠的夏

程序.c

1013956044

最近在研究陀螺仪和加速度传感器的融合，卡尔曼滤波实在搞不定，打算用互补滤波，从网上搜到的资料研究了一下
互补滤波的核心公式：

                          angle = A*(angle + gyro*dt) + (1-A)*(x_acc);

其中gyro和x_acc分别为陀螺仪和加速度传感器AD采集出来的值归一化(如何进行归一化也正在研究，跪求大神指导)后的结果。

dt应该为函数执行的周期。
但如何也不明白A参数是如何确定的
查资料A和dt有如下关系：
                                A= tau / (tau +dt);
tau 为时间常数

于是我不明白tau是如何确定的呢？他和dt 有什么关系呢？

还有就是那个dt,到底是AD采样周期还是执行周期呢。

过去式

1013956044 发表于 2014-12-13 21:27
最近在研究陀螺仪和加速度传感器的融合，卡尔曼滤波实在搞不定，打算用互补滤波，从网上搜到的资料研究了一 ...

dt是采样周期、时间常数t你可以自己去凑。

1013956044

过去式 发表于 2014-12-14 12:57
dt是采样周期、时间常数t你可以自己去凑。

能不能解释下公式

过去式

1013956044 发表于 2014-12-15 18:38
能不能解释下公式

你看一下这个

1013956044

过去式 发表于 2014-12-18 14:25
你看一下这个

angle +=P_enc*enc2_anglespeed*dt*0.0001+ (1-P_enc)*(acc_angle-angle);
师兄留下来的，百思不得姐啊，只知道是互补算法

过去式

1013956044 发表于 2014-12-18 21:16
angle +=P_enc*enc2_anglespeed*dt*0.0001+ (1-P_enc)*(acc_angle-angle);
师兄留下来的，百思不得姐啊， ...

你师兄没给你解释？这个其实都差不多，你把有关互补的都发上来，我看看能不能帮你解释一下

1013956044

过去式 发表于 2014-12-19 12:33
你师兄没给你解释？这个其实都差不多，你把有关互补的都发上来，我看看能不能帮你解释一下

Ang_Acc整体趋势的正确使其可以准确反映角度变化的正确走势，于是将其与Ang_IGyro做差就得到了一个误差值，这个值就是陀螺仪此时的静误差，将这个误差值修复到Ang_IGyro上，得出来的角度值就相对准确了，这就是互补滤波的思路

下面是我的程序，直接用偏差修复角度的，整体来说还是比较简单的：

//互补滤波

Ang_Error=Ang_Acc-Ang_IGyro(陀螺仪);

Ang_IGyro=Ang_IGyro+Ang_Error*A;

A=0.0005

//绿的那个参数越大曲线跟随效应越好但是毛刺越多需要调

Ang=Ang_IGyro;

PS:互补滤波在1ms中断函数中，实时得出角度

得到Ang_Error后，将其乘以一个系数修复到Ang_IGyro上，就可以消除Ang_IGyro的静差了，那个系数越小，曲线修复的越少，也就是说反应越迟钝，曲线越圆滑，当车子有突然变化时，角度不是很突然的改变，而是慢慢的变化过去，当系数较大时候，最终曲线更加跟随加速度曲线，当车子有突然变化时候，角度也会很快的改变过去，但有时候就像加速度计一样有一些毛刺，大家做的时候可以试试这两种特点的曲线，建议这个值还是小一些，这样曲线会更加的平稳，陀螺仪积分得到的角度成分更大，而这个算法是一个积累过程，在上电时只要加速度计得出的角度也在0附近(将车子摆到平衡位置上电即可)，很快曲线就可以跟随过去。如果上电时没有摆正，在上电的一瞬间加速度得出的角度是其真实角度，而陀螺仪从0开始积分，得出的角度是不准确的，需要互补滤波中加速度计的成分逐渐将其修复过去，此处大家可以通过上位机看看波形我就不一一截图了，还是比较简单的