【学习心得】MPU6050数据处理实验二——尝试卡尔曼
本帖最后由 葱拌豆腐 于 2013-4-10 09:03 编辑================2013-04-10更新==========
昨天晚上回去试了一下“P00=(1-Kg*H)*P10*(1-Kg*H)+Kg*R*Kg;”的效果，没有发现任何不同，Kg在第四个循环后就已经收敛到了0.42。
====================================================
自从接触自衡小车开始就发现卡尔曼是个必须过的坎儿，翻遍了坛子里的和卡尔曼有关的帖子，中文的外文的资料看了一大堆，还是没有真正搞明白卡尔曼的原理，没到这个时候心里那个后悔啊，当初在学校为啥不好好学习呢，早知今日要做自衡小车，当初真应该好好学习一下。好在最近照葫芦画瓢的写了一个简单的融合算法，先贴出来，自己做实验时，感觉效果很一般，没有预期中的牛逼，还在进一步研究，下面先贴代码。
#include <I2Cdev.h>
#include <MPU6050.h>
#include <Wire.h>
float ax,az,gy;//get acceleration_x,acceleration_z,rotation_y
long OffsetGy,OffsetSum;
float AngleAcc,AngleRotation,AngleMerge;//angel computed by acceleration and rotation
long LastTime,NowTime,TimeSpan,SampleTime;//
//===========kalman filter parameter define============
float A,B,H,Q,R,P00,P10,X00,X10,Z,U,Kg;
MPU6050 mpu;
void setup()
{
//============initial kalman parameter======
A=1.0;
H=1.0;
Q=0.003;
R=0.01;
P00=5.0;
P10=0.0;
X00=0.0;
//===============end initial===========
Wire.begin();
Serial.begin(9600);
Serial.println("start");
mpu.initialize();
Serial.println(mpu.getRotationY());
Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
//compute the gyroscope offset automatic
for(int i=0;i<200;i++)
{
Serial.println(mpu.getRotationY());
OffsetSum=OffsetSum+mpu.getRotationY();
delay(50);
}
OffsetGy=OffsetSum/200;
pid_SP=0.0;
Kp=10.0;
Kd=2.0;
Ki=0.0;
}
void loop()
{
NowTime=millis();
TimeSpan=NowTime-LastTime;
LastTime=NowTime;
//=================kalman filter==========================
ax=mpu.getAccelerationX()/16384.00;
az=mpu.getAccelerationZ()/16384.00;
AngleAcc=(-1.0)*atan2(ax,az)*180/PI;
gy=(mpu.getRotationY()-OffsetGy)/131.00;
AngleRotation=AngleRotation+gy*TimeSpan/1000.00;
//=============kalman calculate============
B=TimeSpan/1000.0;
U=gy;
Z=AngleAcc;
//一下按照卡尔曼滤波的五个公式写的，融合角度作为状态，陀螺仪的角速度作为控制量，加速度的角度作为观测值
//所以A是1，B是采样周期
X10=A*X00+B*U; //=======formula 1
P10=A*P00*(A)+Q;//=============formula 2
Kg=P10*(H)/(H*P10*(H)+R);//=======formula 3
X00=X10+Kg*(Z-H*X10);//========formula 4
P00=(1-Kg*H)*P10;//=============formula 5
//P00=(1-Kg*H)*P10*(1-Kg*H)+Kg*R*Kg;//======根据维基百科的说法，当Kg不是最优时采用这个公式
AngleMerge=X00;
Serial.print(AngleRotation);
Serial.print(",");
Serial.print(AngleMerge);
Serial.print(",");
Serial.print(AngleAcc);
Serial.print(",");
Serial.print(0.8*AngleRotation+0.2*AngleAcc);
Serial.print(",");
Serial.print(P00);
Serial.print(",");
Serial.println(Kg);
delay(100);
}
监视Kg的变化，发现几个周期之内就不再变化了，滤波效果图如下：
对卡尔曼滤波的研究还在继续，后面会继续更新本文。
mpu6050不需要kalman johnsonzzd 发表于 2013-4-9 20:27 static/image/common/back.gif
mpu6050不需要kalman
没明白啥意思？ johnsonzzd 发表于 2013-4-9 20:27 static/image/common/back.gif
mpu6050不需要kalman
同问，为啥用不着？ 不需要kalman滤波的原因是MPU6050里面同时集成了加速度和角速度传感器，经过内部的DMP（Digital Motion Processor）处理后自动生成角度值。我们直接从它的内部寄存器把角度读出来就行了。 楼上的，我找了DATASHEET，就是找不到DMP如何使用，请指点一二吧......... johnsonzzd 发表于 2013-5-3 14:40 static/image/common/back.gif
不需要kalman滤波的原因是MPU6050里面同时集成了加速度和角速度传感器，经过内部的DMP（Digital Motion Pro ...
我找了DATASHEET，就是找不到DMP如何使用，请指点一二吧 Microsoft 发表于 2013-5-4 10:01 static/image/common/back.gif
我找了DATASHEET，就是找不到DMP如何使用，请指点一二吧
DMP细节是不公开的。 johnsonzzd 发表于 2013-5-4 14:33 static/image/common/back.gif
DMP细节是不公开的。
不公开的话怎么使用啊？求指导啊 johnsonzzd 发表于 2013-4-9 20:27 static/image/common/back.gif
mpu6050不需要kalman
DMP简直就是垃圾，做的数据极其不稳定，还有很大的地方需要改进。 笨笨 发表于 2013-5-6 08:20 static/image/common/back.gif
DMP简直就是垃圾，做的数据极其不稳定，还有很大的地方需要改进。
你说说怎么个垃圾法。我用还没发现特别大的问题，只是刚上电后要收敛几秒钟。 johnsonzzd 发表于 2013-5-3 14:40 static/image/common/back.gif
不需要kalman滤波的原因是MPU6050里面同时集成了加速度和角速度传感器，经过内部的DMP（Digital Motion Pro ...
你好johnsonzzd，我想请教你个问题
[*]我现在有两个范例脚本
[*]MPU6050_DMP6.ino
[*]MPU6050_raw.ino
请问这两个脚本给出的值都通过DMP出来的吗？[*]我在运行MPU6050_DMP6.ino时，发现不是很稳定，经常读到xyz旋转值是0，0，0 ，但偶尔会成功读到正常的旋转值，你遇到过这样的情况吗？
[*]我想利用 做位移的运算，因此我开了一个贴 记录了我的一些想法和问题， 对于MPU6050的加速度运算结果，我想知道是沿反方向给负值？还是减速的时候给负值呢？
小人 发表于 2013-5-20 00:30 static/image/common/back.gif
你好johnsonzzd，我想请教你个问题
[*]我现在有两个范例脚本
1、只有第二个用DMP
2、没有
3、减速就是反方向加速，当然是负的
另外，你搞的怎么样了 笨笨 发表于 2013-5-6 08:20 static/image/common/back.gif
DMP简直就是垃圾，做的数据极其不稳定，还有很大的地方需要改进。
大哥可否将DMP的相关资料分享一下 楼主，我是个初学者，我想请问这个图是怎么搞出来的？