OpenEdv-开源电子网

 找回密码
 立即注册
正点原子新作:阿波罗STM32F767&F429&探索者STM32F4开发板&赶快来下载资料哦。

查看: 4715|回复: 94

分享我的MPU6050+卡尔曼+一阶互补+二阶互补滤波

  [复制链接]

  离线 

23

主题

258

帖子

0

精华

金牌会员

Rank: 6Rank: 6

积分
1506
金钱
1506
注册时间
2015-8-17
在线时间
271 小时
发表于 2016-7-22 10:20:03 | 显示全部楼层 |阅读模式
本帖最后由 wszdxmh 于 2016-8-23 15:12 编辑

不说,上图
X-Kalman.png
X-Kalman滤波
Y-Kalman.png
Y-Kalman滤波
Z-Kalman.png
Z-Kalman滤波,这个滤得不是很好。
X-Thr.png
X轴三种滤波对比图
Big.png
放大图
一阶互补跟随快,但是抗干扰也会差一点,其他两个要等几个周期才会跟随比较好,应该是要更新参数。
上代码。
STM32F103VModel.zip (409.82 KB, 下载次数: 1733)
做事的原则:
1.每个问题重复三遍、研究三遍后再提问,直接得到答案的人什么都没学会。
2.做事要有始有终,感谢那些帮助自己解决问题的人,把解决的方法总结起来。
回复

使用道具 举报

  离线 

23

主题

258

帖子

0

精华

金牌会员

Rank: 6Rank: 6

积分
1506
金钱
1506
注册时间
2015-8-17
在线时间
271 小时
 楼主| 发表于 2016-7-25 14:54:32 | 显示全部楼层
橘子先生 发表于 2016-7-25 14:36
嗯,当加速度是1g的时候输出值是16384对吧?但是有点不明白你程序中加速度处理中
if (Accel_x

谢谢指正!应该是我打错了,我当时没注意。不过也能跑。。。
做事的原则:
1.每个问题重复三遍、研究三遍后再提问,直接得到答案的人什么都没学会。
2.做事要有始有终,感谢那些帮助自己解决问题的人,把解决的方法总结起来。
回复 支持 1 反对 0

使用道具 举报

  离线 

23

主题

258

帖子

0

精华

金牌会员

Rank: 6Rank: 6

积分
1506
金钱
1506
注册时间
2015-8-17
在线时间
271 小时
 楼主| 发表于 2016-7-22 15:51:00 | 显示全部楼层
只见下载,不见顶贴。。。
做事的原则:
1.每个问题重复三遍、研究三遍后再提问,直接得到答案的人什么都没学会。
2.做事要有始有终,感谢那些帮助自己解决问题的人,把解决的方法总结起来。
回复 支持 1 反对 0

使用道具 举报

  离线 

1

主题

34

帖子

0

精华

初级会员

Rank: 2

积分
93
金钱
93
注册时间
2016-6-10
在线时间
19 小时
发表于 2016-7-22 16:31:16 | 显示全部楼层
有没有输出欧拉角比较准确不漂移的算法
还有线性位移
这些算法才是真正有用的算法
回复 支持 反对

使用道具 举报

  离线 

23

主题

258

帖子

0

精华

金牌会员

Rank: 6Rank: 6

积分
1506
金钱
1506
注册时间
2015-8-17
在线时间
271 小时
 楼主| 发表于 2016-7-22 16:49:10 | 显示全部楼层
本帖最后由 wszdxmh 于 2016-7-22 16:53 编辑
达克罗德 发表于 2016-7-22 16:31
有没有输出欧拉角比较准确不漂移的算法
还有线性位移
这些算法才是真正有用的算法

MPU6050模块8块5一个,你还想怎么样?我同学在质检局工作,人家的陀螺仪准到不行。我现在知道的也就只有这几种算法,跟我一起坐等大神回复吧。
做事的原则:
1.每个问题重复三遍、研究三遍后再提问,直接得到答案的人什么都没学会。
2.做事要有始有终,感谢那些帮助自己解决问题的人,把解决的方法总结起来。
回复 支持 反对

使用道具 举报

  离线 

0

主题

44

帖子

0

精华

中级会员

Rank: 3Rank: 3

积分
365
金钱
365
注册时间
2014-12-13
在线时间
86 小时
发表于 2016-7-23 18:55:36 | 显示全部楼层
顶顶顶,学习一下
回复 支持 反对

使用道具 举报

  离线 

6

主题

37

帖子

0

精华

中级会员

Rank: 3Rank: 3

积分
406
金钱
406
注册时间
2016-1-26
在线时间
49 小时
发表于 2016-7-23 21:45:12 | 显示全部楼层
谢谢              
回复 支持 反对

使用道具 举报

  离线 

7

主题

42

帖子

0

精华

中级会员

Rank: 3Rank: 3

积分
325
金钱
325
注册时间
2016-2-28
在线时间
48 小时
发表于 2016-7-23 21:54:26 | 显示全部楼层
问一下楼主u16类型的加速度计值和陀螺仪值在代入互补滤波的计算公式之前需要处理吗?如果需要那要怎么处理?
回复 支持 反对

使用道具 举报

  离线 

23

主题

258

帖子

0

精华

金牌会员

Rank: 6Rank: 6

积分
1506
金钱
1506
注册时间
2015-8-17
在线时间
271 小时
 楼主| 发表于 2016-7-23 22:27:02 | 显示全部楼层
橘子先生 发表于 2016-7-23 21:54
问一下楼主u16类型的加速度计值和陀螺仪值在代入互补滤波的计算公式之前需要处理吗?如果需要那要怎么处理 ...

需要用反切计算为角度,因为输入的参数为速度和角度,所以要把采集的角速度转换成角度。你可以下载我的工程看看,里面都有。
做事的原则:
1.每个问题重复三遍、研究三遍后再提问,直接得到答案的人什么都没学会。
2.做事要有始有终,感谢那些帮助自己解决问题的人,把解决的方法总结起来。
回复 支持 反对

使用道具 举报

  离线 

7

主题

42

帖子

0

精华

中级会员

Rank: 3Rank: 3

积分
325
金钱
325
注册时间
2016-2-28
在线时间
48 小时
发表于 2016-7-24 20:00:12 | 显示全部楼层
wszdxmh 发表于 2016-7-23 22:27
需要用反切计算为角度,因为输入的参数为速度和角度,所以要把采集的角速度转换成角度。你可以下载我的工 ...

啊,好的,我回去试试,谢啦
回复 支持 反对

使用道具 举报

  离线 

14

主题

46

帖子

0

精华

中级会员

Rank: 3Rank: 3

积分
219
金钱
219
注册时间
2016-4-9
在线时间
70 小时
发表于 2016-7-24 21:05:37 | 显示全部楼层
顶顶顶
回复 支持 反对

使用道具 举报

  离线 

7

主题

42

帖子

0

精华

中级会员

Rank: 3Rank: 3

积分
325
金钱
325
注册时间
2016-2-28
在线时间
48 小时
发表于 2016-7-25 13:40:41 | 显示全部楼层
360截图20160725133847093.jpg 360截图20160725133903069.jpg 问下楼主这里的分辨率是什么意思?跟实际测得的数值有什么联系?
回复 支持 反对

使用道具 举报

  离线 

23

主题

258

帖子

0

精华

金牌会员

Rank: 6Rank: 6

积分
1506
金钱
1506
注册时间
2015-8-17
在线时间
271 小时
 楼主| 发表于 2016-7-25 14:10:15 | 显示全部楼层
橘子先生 发表于 2016-7-25 13:40
问下楼主这里的分辨率是什么意思?跟实际测得的数值有什么联系?

上面写得很清楚啊。
这个跟后面的算正负号有关系。
做事的原则:
1.每个问题重复三遍、研究三遍后再提问,直接得到答案的人什么都没学会。
2.做事要有始有终,感谢那些帮助自己解决问题的人,把解决的方法总结起来。
回复 支持 反对

使用道具 举报

  离线 

7

主题

42

帖子

0

精华

中级会员

Rank: 3Rank: 3

积分
325
金钱
325
注册时间
2016-2-28
在线时间
48 小时
发表于 2016-7-25 14:36:33 | 显示全部楼层
wszdxmh 发表于 2016-7-25 14:10
上面写得很清楚啊。
这个跟后面的算正负号有关系。

嗯,当加速度是1g的时候输出值是16384对吧?但是有点不明白你程序中加速度处理中
if (Accel_x<32764) x = Accel_x / 16384;
        else              x = 1 - (Accel_x - 49152) / 16384;
里面的32764和49152是什么意思,如果是2g的话,比较值不应该是32768吗?那个49152是什么还是看不懂
回复 支持 反对

使用道具 举报

  离线 

7

主题

42

帖子

0

精华

中级会员

Rank: 3Rank: 3

积分
325
金钱
325
注册时间
2016-2-28
在线时间
48 小时
发表于 2016-7-25 14:56:44 | 显示全部楼层
本帖最后由 橘子先生 于 2016-7-25 14:57 编辑
wszdxmh 发表于 2016-7-25 14:54
谢谢指正!应该是我打错了,我当时没注意。不过也能跑。。。

那那个49152是什么?而且我看好多人写的程序都是32764不是32768
回复 支持 反对

使用道具 举报

  离线 

1

主题

14

帖子

0

精华

初级会员

Rank: 2

积分
72
金钱
72
注册时间
2016-7-10
在线时间
6 小时
发表于 2016-7-25 15:59:26 | 显示全部楼层
顶顶顶
回复 支持 反对

使用道具 举报

  离线 

23

主题

258

帖子

0

精华

金牌会员

Rank: 6Rank: 6

积分
1506
金钱
1506
注册时间
2015-8-17
在线时间
271 小时
 楼主| 发表于 2016-7-25 16:40:48 | 显示全部楼层
橘子先生 发表于 2016-7-25 14:56
那那个49152是什么?而且我看好多人写的程序都是32764不是32768

应该是32768,因为是65535/2,他们写成那样我也不知道是为什么。
做事的原则:
1.每个问题重复三遍、研究三遍后再提问,直接得到答案的人什么都没学会。
2.做事要有始有终,感谢那些帮助自己解决问题的人,把解决的方法总结起来。
回复 支持 反对

使用道具 举报

  离线 

7

主题

42

帖子

0

精华

中级会员

Rank: 3Rank: 3

积分
325
金钱
325
注册时间
2016-2-28
在线时间
48 小时
发表于 2016-7-25 18:02:36 | 显示全部楼层
wszdxmh 发表于 2016-7-25 16:40
应该是32768,因为是65535/2,他们写成那样我也不知道是为什么。

哦,好吧。。
回复 支持 反对

使用道具 举报

  离线 

23

主题

258

帖子

0

精华

金牌会员

Rank: 6Rank: 6

积分
1506
金钱
1506
注册时间
2015-8-17
在线时间
271 小时
 楼主| 发表于 2016-7-25 19:22:41 | 显示全部楼层

我很怀疑是第一个共享代码的人写错了,后面的人都错了。
做事的原则:
1.每个问题重复三遍、研究三遍后再提问,直接得到答案的人什么都没学会。
2.做事要有始有终,感谢那些帮助自己解决问题的人,把解决的方法总结起来。
回复 支持 反对

使用道具 举报

  离线 

7

主题

42

帖子

0

精华

中级会员

Rank: 3Rank: 3

积分
325
金钱
325
注册时间
2016-2-28
在线时间
48 小时
发表于 2016-7-25 22:03:18 | 显示全部楼层
wszdxmh 发表于 2016-7-25 19:22
我很怀疑是第一个共享代码的人写错了,后面的人都错了。

哈哈,有可能
回复 支持 反对

使用道具 举报

  离线 

10

主题

105

帖子

0

精华

中级会员

Rank: 3Rank: 3

积分
266
金钱
266
注册时间
2016-5-25
在线时间
59 小时
发表于 2016-7-26 21:43:35 | 显示全部楼层
楼主您好~我移植的您的代码,可是经过卡尔曼计算出来之后的值,为什么一直都是-1呢?哪里出错了?
                        mpu6050_send_data(Angle_x_temp,Angle_X_Final,(Angle_y_temp),(Angle_Y_Final),(Angle_z_temp),(Angle_Z_Final));
就是这个函数中,Angle_X_Final,Angle_Y_Final,Angle_Z_Final,的值一直是-1,其他的三个值正常!
回复 支持 反对

使用道具 举报

  离线 

23

主题

258

帖子

0

精华

金牌会员

Rank: 6Rank: 6

积分
1506
金钱
1506
注册时间
2015-8-17
在线时间
271 小时
 楼主| 发表于 2016-7-26 23:39:33 | 显示全部楼层
王轩 发表于 2016-7-26 21:43
楼主您好~我移植的您的代码,可是经过卡尔曼计算出来之后的值,为什么一直都是-1呢?哪里出错了?
                        mpu6 ...

这个我就不知道你移植哪里出错了。我没遇过这个情况。
做事的原则:
1.每个问题重复三遍、研究三遍后再提问,直接得到答案的人什么都没学会。
2.做事要有始有终,感谢那些帮助自己解决问题的人,把解决的方法总结起来。
回复 支持 反对

使用道具 举报

  离线 

10

主题

54

帖子

0

精华

中级会员

Rank: 3Rank: 3

积分
361
金钱
361
注册时间
2016-6-20
在线时间
101 小时
发表于 2016-7-27 08:50:42 | 显示全部楼层
为什么 用了DMP 还用卡尔曼来算啊  卡尔曼出来的也是欧拉角?  看你的图片仅仅是对原始数据的滤波啊,但是如果直接用DMP,是不能对原始数据进行任何处理的啊,直接FIFO读数据了~
回复 支持 反对

使用道具 举报

  离线 

23

主题

258

帖子

0

精华

金牌会员

Rank: 6Rank: 6

积分
1506
金钱
1506
注册时间
2015-8-17
在线时间
271 小时
 楼主| 发表于 2016-7-27 09:19:28 | 显示全部楼层
chuck__chee 发表于 2016-7-27 08:50
为什么 用了DMP 还用卡尔曼来算啊  卡尔曼出来的也是欧拉角?  看你的图片仅仅是对原始数据的滤波啊,但是 ...

你去看看那些开源四轴的代码,基本上都是自己滤波,有些第一代用DMP,第二代更新了之后也用改成自己滤波。至于具体的,我也没研究过,但是大家一致选择这种方案,就一定有他的道理。
做事的原则:
1.每个问题重复三遍、研究三遍后再提问,直接得到答案的人什么都没学会。
2.做事要有始有终,感谢那些帮助自己解决问题的人,把解决的方法总结起来。
回复 支持 反对

使用道具 举报

  离线 

10

主题

105

帖子

0

精华

中级会员

Rank: 3Rank: 3

积分
266
金钱
266
注册时间
2016-5-25
在线时间
59 小时
发表于 2016-7-27 10:43:15 | 显示全部楼层
wszdxmh 发表于 2016-7-26 23:39
这个我就不知道你移植哪里出错了。我没遇过这个情况。

好吧~我再挣扎挣扎
回复 支持 反对

使用道具 举报

  离线 

0

主题

26

帖子

0

精华

初级会员

Rank: 2

积分
156
金钱
156
注册时间
2016-7-5
在线时间
41 小时
发表于 2016-7-27 15:56:08 | 显示全部楼层
顶顶,学习学习学习学习.....
回复 支持 反对

使用道具 举报

  离线 

33

主题

228

帖子

0

精华

高级会员

Rank: 4

积分
661
金钱
661
注册时间
2016-4-20
在线时间
125 小时
发表于 2016-7-27 16:25:59 | 显示全部楼层
wszdxmh 发表于 2016-7-22 15:51
只见下载,不见顶贴。。。

楼主你研究pid是哪一个方面的,我这边也会抽时间研究,不过我们项目是研究pid参数的自整定不知道你有没有好的思路。我们用的是改进的Z-N法。
回复 支持 反对

使用道具 举报

  离线 

23

主题

258

帖子

0

精华

金牌会员

Rank: 6Rank: 6

积分
1506
金钱
1506
注册时间
2015-8-17
在线时间
271 小时
 楼主| 发表于 2016-7-27 17:09:10 | 显示全部楼层
czdspeed 发表于 2016-7-27 16:25
楼主你研究pid是哪一个方面的,我这边也会抽时间研究,不过我们项目是研究pid参数的自整定不知道你有没有 ...

我现在只是在做静态的,自整定还没研究。到时底做到了这部分希望可以交流一下。
做事的原则:
1.每个问题重复三遍、研究三遍后再提问,直接得到答案的人什么都没学会。
2.做事要有始有终,感谢那些帮助自己解决问题的人,把解决的方法总结起来。
回复 支持 反对

使用道具 举报

  离线 

46

主题

1426

帖子

2

精华

论坛大神

Rank: 7Rank: 7Rank: 7

积分
2080
金钱
2080
注册时间
2011-1-25
在线时间
47 小时
发表于 2016-7-27 18:39:58 | 显示全部楼层
顶一下。
回复 支持 反对

使用道具 举报

  离线 

10

主题

105

帖子

0

精华

中级会员

Rank: 3Rank: 3

积分
266
金钱
266
注册时间
2016-5-25
在线时间
59 小时
发表于 2016-7-27 19:06:28 | 显示全部楼层
wszdxmh 发表于 2016-7-26 23:39
这个我就不知道你移植哪里出错了。我没遇过这个情况。

突然之间发现了~起始代码都正常的,知识我用的stc单片机,要等2分多种才可以!波形才完全正常~谢谢啦!
顺便在问一下,经过卡尔曼计算出来的三个姿态角可以直接用了吗?
回复 支持 反对

使用道具 举报

  离线 

23

主题

258

帖子

0

精华

金牌会员

Rank: 6Rank: 6

积分
1506
金钱
1506
注册时间
2015-8-17
在线时间
271 小时
 楼主| 发表于 2016-7-27 19:51:17 | 显示全部楼层
王轩 发表于 2016-7-27 19:06
突然之间发现了~起始代码都正常的,知识我用的stc单片机,要等2分多种才可以!波形才完全正常~谢谢啦!
...

我是直接用的。
做事的原则:
1.每个问题重复三遍、研究三遍后再提问,直接得到答案的人什么都没学会。
2.做事要有始有终,感谢那些帮助自己解决问题的人,把解决的方法总结起来。
回复 支持 反对

使用道具 举报

  离线 

10

主题

105

帖子

0

精华

中级会员

Rank: 3Rank: 3

积分
266
金钱
266
注册时间
2016-5-25
在线时间
59 小时
发表于 2016-7-27 19:54:13 | 显示全部楼层
wszdxmh 发表于 2016-7-27 19:51
我是直接用的。

嗯   谢谢啦~
回复 支持 反对

使用道具 举报

  离线 

8

主题

223

帖子

0

精华

资深版主

Rank: 8Rank: 8

积分
644
金钱
644
注册时间
2015-2-7
在线时间
142 小时
发表于 2016-7-27 20:33:39 | 显示全部楼层
666,谢谢分享
回复 支持 反对

使用道具 举报

  离线 

0

主题

1

帖子

0

精华

新手上路

Rank: 1

积分
36
金钱
36
注册时间
2016-7-28
在线时间
5 小时
发表于 2016-7-28 09:17:20 | 显示全部楼层
最近正在研究卡尔曼滤波,谢谢分享。
回复 支持 反对

使用道具 举报

  离线 

2

主题

4

帖子

0

精华

新手上路

Rank: 1

积分
39
金钱
39
注册时间
2015-9-1
在线时间
4 小时
发表于 2016-7-28 09:46:07 | 显示全部楼层
可以加还好友交流吗?2997553568
回复 支持 反对

使用道具 举报

  离线 

7

主题

108

帖子

0

精华

高级会员

Rank: 4

积分
546
金钱
546
注册时间
2016-7-19
在线时间
89 小时
发表于 2016-7-28 10:39:40 | 显示全部楼层
最近正在学习这个。顶一下!!!!
回复 支持 反对

使用道具 举报

  离线 

0

主题

2

帖子

0

精华

新手上路

Rank: 1

积分
37
金钱
37
注册时间
2016-3-3
在线时间
7 小时
发表于 2016-8-1 11:06:56 | 显示全部楼层
好东西就需要分享!
回复 支持 反对

使用道具 举报

  离线 

37

主题

514

帖子

1

精华

金牌会员

Rank: 6Rank: 6

积分
1249
金钱
1249
注册时间
2011-11-27
在线时间
94 小时
发表于 2016-8-1 11:51:27 | 显示全部楼层
好贴,谢谢楼主分享,收藏了。
永远保持一颗学习的心态。
回复 支持 反对

使用道具 举报

  离线 

5

主题

40

帖子

0

精华

中级会员

Rank: 3Rank: 3

积分
453
金钱
453
注册时间
2016-3-29
在线时间
102 小时
发表于 2016-8-1 13:14:54 | 显示全部楼层
顶一个,楼主偶看啦
回复 支持 反对

使用道具 举报

  离线 

0

主题

1

帖子

0

精华

新手上路

Rank: 1

积分
7
金钱
7
注册时间
2016-8-5
在线时间
0 小时
发表于 2016-8-5 23:27:40 | 显示全部楼层
因为使用DMP速度太慢了,用DMP最大速率才200HZ
回复 支持 反对

使用道具 举报

  离线 

1

主题

6

帖子

0

精华

新手上路

Rank: 1

积分
48
金钱
48
注册时间
2016-7-21
在线时间
10 小时
发表于 2016-8-6 01:22:32 | 显示全部楼层
楼主大神,请问你的源码是直接将GY-521对应连接到PA0与PA1上,之后串口1(PA9,PA10)直接发送给上位机是么?本人初学,请谅解一些很愚笨的问题,我按照我所说的连了线了,之后也下载进去了,我试了几次发现在while(1)之前的那while就卡住了,求解。谢谢您。
回复 支持 反对

使用道具 举报

  离线 

23

主题

258

帖子

0

精华

金牌会员

Rank: 6Rank: 6

积分
1506
金钱
1506
注册时间
2015-8-17
在线时间
271 小时
 楼主| 发表于 2016-8-6 10:06:41 | 显示全部楼层
方向很重要 发表于 2016-8-6 01:22
楼主大神,请问你的源码是直接将GY-521对应连接到PA0与PA1上,之后串口1(PA9,PA10)直接发送给上位机是么 ...

如果是卡在DMP的初始化的话,请把模块放平再试一次,之前我就是这样的。
做事的原则:
1.每个问题重复三遍、研究三遍后再提问,直接得到答案的人什么都没学会。
2.做事要有始有终,感谢那些帮助自己解决问题的人,把解决的方法总结起来。
回复 支持 反对

使用道具 举报

  离线 

5

主题

26

帖子

0

精华

初级会员

Rank: 2

积分
81
金钱
81
注册时间
2016-5-19
在线时间
14 小时
发表于 2016-8-9 18:20:18 | 显示全部楼层
我用printf("%0.2f    %0.2f    %0.2f\r\n",Angle,Angle_ax,Gyro_y);函数分别读取的加速度,角速度和倾角,我发现当我快速的改变板子的倾角的时候,比如快速变化10度,Angle(卡尔曼滤波后的倾角)瞬时变化非常快,可能会瞬间变化几十度然后回到正常角度,而当我缓慢变化10度的时候,Angle变化是正常线性变化到10度,在这两种变化中,Angle_ax(从MPU6050读取的值经过处理后的陀螺仪的Y轴数据)的变化一直都是线性正常的,并且Angle的值特别接近Angle_ax的值
问题:1,我快速改变板子倾角时Angle的变化正常吗?
      2,Angle正常变化的时候是应该与Angle_ax的值相近吗?

现在情况就是,就算我是在减小倾角,只要我快速地改变,它显示的倾角都会先增大再减小,而如果我慢速改变的话,倾角就会缓慢减小而不会出现中间的角度增大


*************读取数据********************
//定义MPU6050内部地址
#define        SMPLRT_DIV                0x19        //陀螺仪采样率 典型值 0X07 125Hz
#define        CONFIG                          0x1A        //低通滤波频率 典型值 0x00
#define        GYRO_CONFIG                0x1B        //陀螺仪自检及测量范围                 典型值 0x18 不自检 2000deg/s
#define        ACCEL_CONFIG        0x1C        //加速度计自检及测量范围及高通滤波频率 典型值 0x01 不自检 2G 5Hz
#define INT_PIN_CFG     0x37
#define INT_ENABLE      0x38
#define INT_STATUS      0x3A    //只读
#define        ACCEL_XOUT_H        0x3B
#define        ACCEL_XOUT_L        0x3C
#define        ACCEL_YOUT_H        0x3D
#define        ACCEL_YOUT_L        0x3E
#define        ACCEL_ZOUT_H        0x3F
#define        ACCEL_ZOUT_L        0x40
#define        TEMP_OUT_H                0x41
#define        TEMP_OUT_L                0x42
#define        GYRO_XOUT_H    0x43
#define        GYRO_XOUT_L                0x44       
#define        GYRO_YOUT_H        0x45
#define        GYRO_YOUT_L                0x46
#define        GYRO_ZOUT_H        0x47
#define        GYRO_ZOUT_L                0x48

//读取寄存器原生数据
           
        MPU6050_Raw_Data.Accel_X = (buf[0]<<8 | buf[1]);
        MPU6050_Raw_Data.Accel_Y = (buf[2]<<8 | buf[3]);
        MPU6050_Raw_Data.Accel_Z = (buf[4]<<8 | buf[5]);
        MPU6050_Raw_Data.Temp =    (buf[6]<<8 | buf[7]);  
        MPU6050_Raw_Data.Gyro_X = (buf[8]<<8 | buf[9]);
        MPU6050_Raw_Data.Gyro_Y = (buf[10]<<8 | buf[11]);
        MPU6050_Raw_Data.Gyro_Z = (buf[12]<<8 | buf[13]);
      
      
        //将原生数据转换为实际值,计算公式跟寄存器的配置有关
        MPU6050_Real_Data.Accel_X = -(float)(MPU6050_Raw_Data.Accel_X)/8192.0;
        MPU6050_Real_Data.Accel_Y = -(float)(MPU6050_Raw_Data.Accel_Y)/8192.0;
        MPU6050_Real_Data.Accel_Z = (float)(MPU6050_Raw_Data.Accel_Z)/8192.0;
              MPU6050_Real_Data.Gyro_X=-(float)(MPU6050_Raw_Data.Gyro_X - gyroADC_X_offset)/65.5;   
        MPU6050_Real_Data.Gyro_Y=-(float)(MPU6050_Raw_Data.Gyro_Y - gyroADC_Y_offset)/65.5;   
        MPU6050_Real_Data.Gyro_Z=(float)(MPU6050_Raw_Data.Gyro_Z - gyroADC_Z_offset)/65.5;   
    }
   


//******卡尔曼参数************
               
const float Q_angle=0.001;  
const float Q_gyro=0.003;
const float R_angle=0.5;
const float dt=0.01;                          //dt为kalman滤波器采样时间;
const char  C_0 = 1;
float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };

/*****************卡尔曼滤波**************************************************/
void Kalman_Filter(float Accel,float Gyro)               
{
        Angle+=(Gyro - Q_bias) * dt; //先验估计
       
        Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分

        Pdot[1]= -PP[1][1];
        Pdot[2]= -PP[1][1];
        Pdot[3]=Q_gyro;
       
        PP[0][0] += Pdot[0] * dt;   // Pk-先验估计误差协方差微分的积分
        PP[0][1] += Pdot[1] * dt;   // =先验估计误差协方差
        PP[1][0] += Pdot[2] * dt;
        PP[1][1] += Pdot[3] * dt;
               
        Angle_err = Accel - Angle;        //zk-先验估计
       
        PCt_0 = C_0 * PP[0][0];
        PCt_1 = C_0 * PP[1][0];
       
        E = R_angle + C_0 * PCt_0;
       
        K_0 = PCt_0 / E;
        K_1 = PCt_1 / E;
       
        t_0 = PCt_0;
        t_1 = C_0 * PP[0][1];

        PP[0][0] -= K_0 * t_0;                 //后验估计误差协方差
        PP[0][1] -= K_0 * t_1;
        PP[1][0] -= K_1 * t_0;
        PP[1][1] -= K_1 * t_1;
               
        Angle        += K_0 * Angle_err;         //后验估计
        Q_bias        += K_1 * Angle_err;         //后验估计
        Gyro_y   = Gyro - Q_bias;         //输出值(后验估计)的微分=角速度

}

******************倾角计算*****************
void Angle_Calculate(void)
{

/****************************加速度****************************************/
       
        Accel_x  =  MPU6050_Real_Data.Accel_X;          //读取X轴加速度
        Angle_ax = Accel_x*1.2*180/3.14;     //弧度转换为度

/****************************角速度****************************************/
       
         Gyro_y = MPU6050_Real_Data.Gyro_Y;             
时间dt,所以此处不用积分
/***************************卡尔曼融合*************************************/
        Kalman_Filter(Angle_ax,Gyro_y);       //卡尔曼滤波计算倾角
       

回复 支持 反对

使用道具 举报

  离线 

23

主题

258

帖子

0

精华

金牌会员

Rank: 6Rank: 6

积分
1506
金钱
1506
注册时间
2015-8-17
在线时间
271 小时
 楼主| 发表于 2016-8-9 20:00:41 | 显示全部楼层
华丽与creazy 发表于 2016-8-9 18:20
我用printf("%0.2f    %0.2f    %0.2f\r\n",Angle,Angle_ax,Gyro_y);函数分别读取的加速度,角速度和倾角, ...

http://www.openedv.com/forum.php ... mp;page=1#pid459593
目测你是移植了一个别人的错误的代码,不行就下载我的程序试试,还有,没有波形图吗?有什么比图更直接?
做事的原则:
1.每个问题重复三遍、研究三遍后再提问,直接得到答案的人什么都没学会。
2.做事要有始有终,感谢那些帮助自己解决问题的人,把解决的方法总结起来。
回复 支持 反对

使用道具 举报

  离线 

5

主题

26

帖子

0

精华

初级会员

Rank: 2

积分
81
金钱
81
注册时间
2016-5-19
在线时间
14 小时
发表于 2016-8-10 13:20:45 | 显示全部楼层
wszdxmh 发表于 2016-8-9 20:00
http://www.openedv.com/forum.php?mod=viewthread&tid=79275&page=1#pid459593
目测你是移植了一个别人 ...

波形图?不知道怎么弄出波形图,软件吗?
回复 支持 反对

使用道具 举报

  离线 

5

主题

26

帖子

0

精华

初级会员

Rank: 2

积分
81
金钱
81
注册时间
2016-5-19
在线时间
14 小时
发表于 2016-8-10 13:50:51 | 显示全部楼层
wszdxmh 发表于 2016-8-9 20:00
http://www.openedv.com/forum.php?mod=viewthread&tid=79275&page=1#pid459593
目测你是移植了一个别人 ...

你能加下我的QQ吗?拜托啦,976675605或者你告诉我你的QQ或者微信什么的,求教
回复 支持 反对

使用道具 举报

  离线 

20

主题

83

帖子

0

精华

中级会员

Rank: 3Rank: 3

积分
403
金钱
403
注册时间
2016-3-17
在线时间
93 小时
发表于 2016-8-10 14:00:59 | 显示全部楼层
         顶
回复 支持 反对

使用道具 举报

  离线 

23

主题

258

帖子

0

精华

金牌会员

Rank: 6Rank: 6

积分
1506
金钱
1506
注册时间
2015-8-17
在线时间
271 小时
 楼主| 发表于 2016-8-10 14:06:47 | 显示全部楼层
华丽与creazy 发表于 2016-8-10 13:50
你能加下我的QQ吗?拜托啦,976675605或者你告诉我你的QQ或者微信什么的,求教

http://www.openedv.com/forum.php ... mp;page=1#pid456169
自己看吧,我在这个帖子里我有说一些使用方法。本贴也有共享代码和上位机软件,你真的没看到?
做事的原则:
1.每个问题重复三遍、研究三遍后再提问,直接得到答案的人什么都没学会。
2.做事要有始有终,感谢那些帮助自己解决问题的人,把解决的方法总结起来。
回复 支持 反对

使用道具 举报

  离线 

5

主题

26

帖子

0

精华

初级会员

Rank: 2

积分
81
金钱
81
注册时间
2016-5-19
在线时间
14 小时
发表于 2016-8-10 17:09:39 | 显示全部楼层
wszdxmh 发表于 2016-8-9 20:00
http://www.openedv.com/forum.php?mod=viewthread&tid=79275&page=1#pid459593
目测你是移植了一个别人 ...

我发现我的这两行代码跟你的不同,我的是
MPU6050_Real_Data.Accel_X = -(float)(MPU6050_Raw_Data.Accel_X)/8192.0;
MPU6050_Real_Data.Gyro_Y=-(float)(MPU6050_Raw_Data.Gyro_Y - gyroADC_Y_offset)/65.5;
你的是        if (Accel_x<32764) x = Accel_x / 16384;
        else              x = 1 - (Accel_x - 49152) / 16384;
        if (Gyro_y<32768) Gyro_y = -(Gyro_y / 16.4);//16.4 LSB/(deg/s)
       
        if (Gyro_y>32768) Gyro_y = +(65535 - Gyro_y) / 16.4;
是因为这两行代码的原因吗?我们除的参数不同
回复 支持 反对

使用道具 举报

  离线 

5

主题

26

帖子

0

精华

初级会员

Rank: 2

积分
81
金钱
81
注册时间
2016-5-19
在线时间
14 小时
发表于 2016-8-10 17:39:02 | 显示全部楼层
wszdxmh 发表于 2016-8-9 20:00
http://www.openedv.com/forum.php?mod=viewthread&tid=79275&page=1#pid459593
目测你是移植了一个别人 ...

我发现我的这两行代码跟你的不同,我的是
MPU6050_Real_Data.Accel_X =-(float)(MPU6050_Raw_Data.Accel_X)/8192.0;
MPU6050_Real_Data.Gyro_Y=-(float)(MPU6050_Raw_Data.Gyro_Y- gyroADC_Y_offset)/65.5;
你的是     if (Accel_x<32764) x =Accel_x / 16384;
         else              x = 1 - (Accel_x - 49152) /16384;
         if(Gyro_y<32768) Gyro_y = -(Gyro_y / 16.4);//16.4 LSB/(deg/s)
         
         if(Gyro_y>32768) Gyro_y = +(65535 - Gyro_y) / 16.4;
是这个原因吗?
这是我卡尔曼滤波后的倾角,我朝一个方向改变倾角,发现它却先向反方向变化,然后才向这个方向变化,并且还会超过,再才到改变后的角度
下图红色是滤波后的倾角,白色是Angle_ax(即加速度计转变为角度后的值),黄色是Gyro_y(即角速度)



WM9$EEJ{WMXNJ2ODS[G-0.png
回复 支持 反对

使用道具 举报


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

本版积分规则


QQ|联系我们|手机版|官方淘宝店|新浪微博|微信公众平台|OpenEdv-开源电子网 ( 粤ICP备12000418号-1 )

GMT+8, 2017-2-24 22:57

Powered by OpenEdv-开源电子网

© 2001-2030 OpenEdv-开源电子网

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