加速度数据滑动滤波莫名错误！求救！
本帖最后由 zzipeng 于 2014-11-15 16:40 编辑如题，我现在弄四轴代码。姿态已经初步解算出来。但是遇到一个很奇怪的问题。即加速度数据平滑滤波时，输出的数据莫名变大。先上代码：
#define gyro_denominator 3753.15f //陀螺仪灵敏度
#define acc_denominator 836.943f //加计灵敏度8192/9.788=836.943 广州重力加速度 9.788m/s^2
int16_t G_X,G_Y,G_Z,A_X,A_Y,A_Z;//减去零偏后的数据
int16_t ACC_Axis_X,ACC_Axis_Y,ACC_Axis_Z;//加计滑动滤波后输出的数据
float IMU_GYRO_X,IMU_GYRO_Y,IMU_GYRO_Z;//角速度rad/s
float IMU_ACC_X,IMU_ACC_Y,IMU_ACC_Z;//加速度m/s^2
。。。。。。。。
if(Data_Slid_Analys==TRUE)//检测加计平滑滤波标志
{
uint8_t i;
static u8 coun=0;
static int16_t ACC_BUF_X;
static int16_t ACC_BUF_Y;
static int16_t ACC_BUF_Z;
int32_t Tempx,Tempy,Tempz;
ACC_BUF_X = A_X;
ACC_BUF_Y = A_Y;
ACC_BUF_Z = A_Z;
for(i=0;i<20;i++)
{
Tempx+=ACC_BUF_X;
Tempy+=ACC_BUF_Y;
Tempz+=ACC_BUF_Z;
}
ACC_Axis_X=Tempx / 20;//求平均输出
ACC_Axis_Y=Tempy / 20;
ACC_Axis_Z=Tempz / 20;
IMU_ACC_X=ACC_Axis_X / acc_denominator; //将滑动滤波后的加计数据转换成以m/s^2为单位的加速度数据
IMU_ACC_Y=ACC_Axis_Y / acc_denominator;
IMU_ACC_Z=ACC_Axis_Z / acc_denominator;
IMU_GYRO_X=G_X / gyro_denominator; //将陀螺仪数据转换成以rad/s为单位的角速度数据
IMU_GYRO_Y=G_Y / gyro_denominator;//
IMU_GYRO_Z=G_Z / gyro_denominator; //
coun++;
if(coun==20) {coun=0； IMUupdate_Flag=TRUE;//可以解算姿态了}
}
可以看到，代码其实很简单的。就是对减去零偏后的加计数据平滑滤波后转换成加速度输出，陀螺仪数据不作滤波处理，直接转换成角速度输出。问题是我用匿名的上位机对加计平滑滤波后的数据进行显示观看。发现变量ACC_Axis_X明显变大，直接飙到26000！见图1。奇怪的是ACC_Axis_Y，ACC_Axis_Z并无影响。于是我将ACC_Axis_X=Tempx / 20;//求平均输出这语句屏蔽掉；将IMU_ACC_X=ACC_Axis_X / acc_denominator; 这语句中的ACC_Axis_X 改为A_X，即X轴加计数据放弃滤波输出，这时发现轮到ACC_Axis_Y数据发飙了，ACC_Axis_Z无影响！
试过将acc_denominator，gyro_denominator两个参数改为整数，结果并无变化。。。 可以參考我之前寫的
https://github.com/QCopter/QCopterFlightControl/blob/Pre-v2.2/Software/QCopterFC_FlightControl/Program/Algorithms/algorithm_moveAve.c
沒仔細看, 但覺得可能是靈敏度或是益位問題 额 还没有累加到20个数 就去求均值？ asha 发表于 2014-11-15 15:42
额 还没有累加到20个数 就去求均值？
这个代码是局部的。。。实际的代码是这个函数执行20次之后才允许后面姿态解算 john800422 发表于 2014-11-15 15:22
可以參考我之前寫的
https://github.com/QCopter/QCopterFlightControl/blob/Pre-v2.2/Software/QCopterFC_ ...
你给的链接上的代码看了，感觉没什么区别啊。。。原理都一样
能讲一下什么事溢位吗？与溢出有什么不同。。。谢了 应该是硬件的问题。。。。 累加和Tempx临时变量，会不会有时候进入的时候不是0呢？一般我们定义成静态变量，，计算一次之后清0，也就是=20的时候清0. 本帖最后由 zzipeng 于 2014-11-15 20:43 编辑
lovecxm 发表于 2014-11-15 19:11
累加和Tempx临时变量，会不会有时候进入的时候不是0呢？一般我们定义成静态变量，，计算一次之后清0，也就 ...
靠。。按你的说法，将Tempx、Tempy、Tempz三个变量由临时类型改为static类型还真奇迹发生了——问题解决。见图 给你一个平滑滤波的算法。我已经用了10年了，非常好用，比起你那个20点统计一次并求平均的算法要简单很多。这个算法同样也是求平均值滤波算法。
uint8filter_NUM=20; //定义需要滤波数据的个数
Out_AD_Data=(filter_NUM*AD_Data_filter_last+AD_Data))/(filter_NUM+1);
AD_Data_filter_last=Out_AD_Data;
Out_AD_Data是最终输出后的滤波值，也就是你要用的值
AD_Data_filter_last是缓存值，存储上一次得到的滤波数据结果
AD_Data为AD采样到的数据
filter_NUM为定义需要滤波数据的个数
这个算法有个缺点，当前20个数据进入前Out_AD_Data输出数据会偏小很多。但是一旦AD数据超过20个以后,这个平滑滤波系统输出数据就很稳定了，并且不需要每次都采集20个数据后再进行滤波。可以直接一直滤波。 dj1981812 发表于 2014-11-15 21:20
给你一个平滑滤波的算法。我已经用了10年了，非常好用，比起你那个20点统计一次并求平均的算法要简单很多。 ...
谢谢！代码看了，的确很巧妙！{:handshake:} 不客气，好用就行。 你这个不对吧？！temp本来就应该是临时变量，所以你贴的源码没有错误。然后就是，加速度计静止时读数不应该是零而是重力加速度值，所以一般平放时Z轴会有很大的输出。另外确保没有数据类型错误！ 略看了一下代码，发现你Tempx,Tempy,Tempz根本没清零，不飙大数据才奇怪呢。
你用的是均值滤波，连续采集20次后，再求平均值。即在
ACC_Axis_X=Tempx / 20;//求平均输出
ACC_Axis_Y=Tempy / 20;
ACC_Axis_Z=Tempz / 20;
后面加上一下清零代码：
Tempx =0;
Tempy =0;
Tempz =0;
这样就解决你的问题了。目测是这个问题，你可以试一下。
应用在四轴上不太建议用均值滤波，降低频响....... oner 发表于 2014-11-16 10:30
略看了一下代码，发现你Tempx,Tempy,Tempz根本没清零，不飙大数据才奇怪呢。
你用的是均值滤波，连续采集20 ...
已经按照你的意见将Tempx、Tempy、Tempz三个变量由static改为临时变量（去掉static）,然后加上Tempx=0;Tempy=0;Tempz=0;几句代码，结果并无改变。忘记说了，我现在用的代码是Tempx、Tempy、Tempz为static变量，然后Tempx=0;Tempy=0;Tempz=0;这三句代码也是有的。
还有要说明的是，我用的是滑动滤波而不是简单的每采样20次然后求平均值输出。看到 ACC_BUF_X=A_X;这代码中的coun和数组ACC_BUF_X[]为static 变量就知道了。
zzipeng 发表于 2014-11-16 12:15
已经按照你的意见将Tempx、Tempy、Tempz三个变量由static改为临时变量（去掉static）,然后加上Tempx=0;Te ...
哦哦，的确是滑动滤波，我之前没细看误导你了。 dj1981812 发表于 2014-11-15 21:20
给你一个平滑滤波的算法。我已经用了10年了，非常好用，比起你那个20点统计一次并求平均的算法要简单很多。 ...
你这个滤波算法滞后会不会很大，第一个点怎么取呢 yanghuanchun 发表于 2015-4-11 14:39
你这个滤波算法滞后会不会很大，第一个点怎么取呢
当然会有滞后了，要看你怎么设计了，你的滤波计数的多少决定你的延迟时间。 dj1981812 发表于 2014-11-15 21:20
给你一个平滑滤波的算法。我已经用了10年了，非常好用，比起你那个20点统计一次并求平均的算法要简单很多。 ...
这个实际就是加权平均~ 临时变量声明，求和是，变量要初始化为0，不然会错
页:
[1]