极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 74050|回复: 35

【学习心得】MPU6050数据处理实验二——尝试卡尔曼

  [复制链接]
发表于 2013-4-9 10:56:33 | 显示全部楼层 |阅读模式
本帖最后由 葱拌豆腐 于 2013-4-10 09:03 编辑

================2013-04-10更新==========
昨天晚上回去试了一下“P00=(1-Kg*H)*P10*(1-Kg*H)+Kg*R*Kg;”的效果,没有发现任何不同,Kg在第四个循环后就已经收敛到了0.42。
====================================================
自从接触自衡小车开始就发现卡尔曼是个必须过的坎儿,翻遍了坛子里的和卡尔曼有关的帖子,中文的外文的资料看了一大堆,还是没有真正搞明白卡尔曼的原理,没到这个时候心里那个后悔啊,当初在学校为啥不好好学习呢,早知今日要做自衡小车,当初真应该好好学习一下。好在最近照葫芦画瓢的写了一个简单的融合算法,先贴出来,自己做实验时,感觉效果很一般,没有预期中的牛逼,还在进一步研究,下面先贴代码。
[pre lang="arduino" line="1" file="Kalman"]
#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);

}
[/code]

监视Kg的变化,发现几个周期之内就不再变化了,滤波效果图如下:

滤波效果对比

滤波效果对比

对卡尔曼滤波的研究还在继续,后面会继续更新本文。
回复

使用道具 举报

发表于 2013-4-9 20:27:16 | 显示全部楼层
mpu6050不需要kalman
回复 支持 反对

使用道具 举报

 楼主| 发表于 2013-4-9 21:47:43 | 显示全部楼层
johnsonzzd 发表于 2013-4-9 20:27
mpu6050不需要kalman

没明白啥意思?
回复 支持 反对

使用道具 举报

发表于 2013-5-3 00:17:32 | 显示全部楼层
johnsonzzd 发表于 2013-4-9 20:27
mpu6050不需要kalman

同问,为啥用不着?
回复 支持 反对

使用道具 举报

发表于 2013-5-3 14:40:09 | 显示全部楼层
不需要kalman滤波的原因是MPU6050里面同时集成了加速度和角速度传感器,经过内部的DMP(Digital Motion Processor)处理后自动生成角度值。我们直接从它的内部寄存器把角度读出来就行了。
回复 支持 反对

使用道具 举报

发表于 2013-5-3 17:07:48 | 显示全部楼层
楼上的,我找了DATASHEET,就是找不到DMP如何使用,请指点一二吧.........
回复 支持 反对

使用道具 举报

发表于 2013-5-4 10:01:43 | 显示全部楼层
johnsonzzd 发表于 2013-5-3 14:40
不需要kalman滤波的原因是MPU6050里面同时集成了加速度和角速度传感器,经过内部的DMP(Digital Motion Pro ...

我找了DATASHEET,就是找不到DMP如何使用,请指点一二吧
回复 支持 反对

使用道具 举报

发表于 2013-5-4 14:33:34 | 显示全部楼层
Microsoft 发表于 2013-5-4 10:01
我找了DATASHEET,就是找不到DMP如何使用,请指点一二吧

DMP细节是不公开的。
回复 支持 反对

使用道具 举报

发表于 2013-5-4 18:51:29 | 显示全部楼层
johnsonzzd 发表于 2013-5-4 14:33
DMP细节是不公开的。

不公开的话怎么使用啊?求指导啊
回复 支持 反对

使用道具 举报

发表于 2013-5-6 08:20:56 | 显示全部楼层
johnsonzzd 发表于 2013-4-9 20:27
mpu6050不需要kalman

DMP简直就是垃圾,做的数据极其不稳定,还有很大的地方需要改进。
回复 支持 反对

使用道具 举报

发表于 2013-5-6 15:43:20 | 显示全部楼层
笨笨 发表于 2013-5-6 08:20
DMP简直就是垃圾,做的数据极其不稳定,还有很大的地方需要改进。

你说说怎么个垃圾法。我用还没发现特别大的问题,只是刚上电后要收敛几秒钟。
回复 支持 反对

使用道具 举报

发表于 2013-5-20 00:30:51 | 显示全部楼层
johnsonzzd 发表于 2013-5-3 14:40
不需要kalman滤波的原因是MPU6050里面同时集成了加速度和角速度传感器,经过内部的DMP(Digital Motion Pro ...

你好johnsonzzd,我想请教你个问题
  • 我现在有两个范例脚本
    • MPU6050_DMP6.ino
    • MPU6050_raw.ino
    请问这两个脚本给出的值都通过DMP出来的吗?
  • 我在运行MPU6050_DMP6.ino时,发现不是很稳定,经常读到xyz旋转值是0,0,0 ,但偶尔会成功读到正常的旋转值,你遇到过这样的情况吗?
  • 我想利用   做位移的运算,因此我开了一个贴 记录了我的一些想法和问题, 对于MPU6050的加速度运算结果,我想知道是沿反方向给负值?还是减速的时候给负值呢?


回复 支持 反对

使用道具 举报

发表于 2013-7-23 09:33:25 | 显示全部楼层
小人 发表于 2013-5-20 00:30
你好johnsonzzd,我想请教你个问题
  • 我现在有两个范例脚本

  • 1、只有第二个用DMP
    2、没有
    3、减速就是反方向加速,当然是负的
    另外,你搞的怎么样了
    回复 支持 反对

    使用道具 举报

    发表于 2013-8-9 17:43:05 | 显示全部楼层
    笨笨 发表于 2013-5-6 08:20
    DMP简直就是垃圾,做的数据极其不稳定,还有很大的地方需要改进。

    大哥可否将DMP的相关资料分享一下
    回复 支持 反对

    使用道具 举报

    发表于 2013-8-10 21:09:26 | 显示全部楼层
    楼主,我是个初学者,我想请问这个图是怎么搞出来的?
    回复 支持 反对

    使用道具 举报

    您需要登录后才可以回帖 登录 | 注册

    本版积分规则 需要先绑定手机号

    Archiver|联系我们|极客工坊 ( 浙ICP备09023225号-2 )

    GMT+8, 2021-5-9 21:16 , Processed in 0.148112 second(s), 30 queries .

    Powered by Discuz! X3.4 Licensed

    © 2001-2017 Comsenz Inc.

    快速回复 返回顶部 返回列表