按照一个教学贴写了个融合算法，前几秒不错，可后来明显反应滞后
本帖最后由 ShadowWalker 于 2013-8-13 13:53 编辑#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
const float Ac_xs = 16384.0; //重力加速度系数
const float Pa_xs = 131.0; //角速度系数
const float Ra=180/PI ; //弧度转角度系数
doubleG; //重力加速度
unsigned long T_Now=0,T_Last=0; //系统当前时间
int Ac; //读取加速度xyz
int Pa; //读取角速度xyz
float P; //真实角速度
float C_Pa; //角速度漂移xyz
float C_Ac; //加速度计漂移
float Pa_X_Now,Pa_X_Last,Pa_Y_Now,Pa_Y_Last; //积分角速度
float Rg; //过程单位矢量
float A; //真实惯性合力分量 |A|=1
float A_X,A_Y; //x,y与Z轴夹角
float Gr; //重力分量 |G|=1算法输出
float W=10; //Rg陀螺仪滤波权重
void setup()
{
Wire.begin();
Serial.begin(38400);
accelgyro.initialize();
for(int a=0;a<3;a++) //获取角速度偏移
{
for(int i=0;i<50;i++)
{
accelgyro.getMotion6(&Ac, &Ac, &Ac, &Pa, &Pa, &Pa);
C_Pa=C_Pa+Pa;
}
C_Pa=C_Pa/50;
}
C_Ac=-0.1;
}
void loop()
{
CalculateGr();
printdata();
delay(20);
}
void printdata ()
{
Serial.print(acos(Gr)*Ra);Serial.print(",");
Serial.print(acos(A)*Ra);Serial.print(",");
Serial.println(acos(Rg)*Ra);
}
void CalculateGr ()
{
accelgyro.getMotion6(&Ac, &Ac, &Ac, &Pa, &Pa, &Pa);
for(unsigned char a=0;a<3;a++) //处理角速度漂移
{
Pa=Pa-C_Pa;
}
for(unsigned char a=0;a<3;a++) //得到真实加速度和角速度
{
A=Ac/Ac_xs;
P=Pa/Pa_xs;
}
A=A-C_Ac; //处理加速度漂移
G=sqrt(A*A+A*A+A*A);
if(T_Last==0) //第一次
{
for(unsigned char a=0;a<3;a++)
{
for(unsigned char b=0;b<10;b++)
{
accelgyro.getMotion6(&Ac, &Ac, &Ac, &Pa, &Pa, &Pa);
A=A/Ac_xs;
A=A+A;
}
A=A/10.0;
Gr=A;
}
A_X=acos(A);
A_Y=acos(A);
T_Last=micros();
Pa_Y_Last=Pa;
Pa_X_Last=Pa;
}
if(T_Last>0)
{
Pa_X_Now=P;
Pa_Y_Now=P;
T_Now=micros();
A_X=A_X+(Pa_Y_Now+Pa_Y_Last)*(T_Now-T_Last)/2000000.0; //XY轴角度，从陀螺仪积分
A_Y=A_Y+(Pa_X_Now-Pa_X_Last)*(T_Now-T_Last)/2000000.0;
if(abs(G-1)>0.05)
{
Rg=sq(sin(A_Y))/sqrt(1+sq((cos(A_X)))*sq(tan(A_Y)));
Rg=sq(sin(A_X))/sqrt(1+sq((cos(A_Y)))*sq(tan(A_X)));
Rg=sqrt(1-sq(Rg)-sq(Rg));
}
if(abs(G-1)>=0.03) //校正
{
for(unsigned char a=0;a<3;a++)
{
A=A/G;
}
G=sqrt(A*A+A*A+A*A); //此时G化为1
for(unsigned char v=0;v<3;v++)
{
Rg=A/G;
}
A_X=acos(A);
A_Y=acos(A);
}
for(unsigned char b=0;b<3;b++) //互补滤波
{
Gr=(A+Rg*W)/(W+1);
}
G=sqrt(sq(Gr)+sq(Gr)+sq(Gr));
for(unsigned char b=0;b<3;b++) //再次单位化 重力矢量
{
Gr=Gr/G;
}
T_Last=micros();
Pa_X_Last=P;
Pa_Y_Last=P;
}
}
参考的是http://www.geek-workshop.com/forum.php?mod=viewthread&tid=1695
真实效果：http://v.youku.com/v_show/id_XNTk1MzU4NDc2.html
前十来秒还行，时间一长就明显反应滞后。
各位帮忙看看怎么弄。 你是要得到加速度的重力分量?真看不懂,我怎么看出来好象在做"水平校准"........楼下的高手继续吧 拾瑞 发表于 2013-8-13 08:32 static/image/common/back.gif
你是要得到加速度的重力分量?真看不懂,我怎么看出来好象在做"水平校准"........楼下的高手继续吧
嗯，得到重力的分量，就能根据余弦关系得到当前传感器的倾角。 参考的什么资料？？？？ 学慧放弃 发表于 2013-8-13 12:07 static/image/common/back.gif
参考的什么资料？？？？
http://www.geek-workshop.com/forum.php?mod=viewthread&tid=1695 ShadowWalker 发表于 2013-8-13 11:24 static/image/common/back.gif
嗯，得到重力的分量，就能根据余弦关系得到当前传感器的倾角。
我看一下，你的这种算法，是以加速度计为主的！我们通常所用的互补滤波，是以陀螺为主，加速度计辅助！（短时间内角度和角速度我们相信陀螺，加速计出的角度只是用来消除陀螺的积分误差）
拾瑞 发表于 2013-8-13 14:01 static/image/common/back.gif
我看一下，你的这种算法，是以加速度计为主的！我们通常所用的互补滤波，是以陀螺为主，加速度计辅助！（ ...
嗯，你说得对，我也知道111行应该是if(abs(G-1)<=0.03) //校正但那样貌似情况更不好了，郁闷
你手里有现成的测试好用的算法吗，发一个我学习学习 ShadowWalker 发表于 2013-8-13 12:09 static/image/common/back.gif
http://www.geek-workshop.com/forum.php?mod=viewthread&tid=1695
那个资料我早就打印了一份，还是外国资料牛，国内简直弱爆了
页:
[1]