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

wszdxmh · 发表于 2016-7-22 10:20:03

本帖最后由 wszdxmh 于 2016-8-23 15:12 编辑

不说，上图

X-Kalman滤波

Y-Kalman滤波

Z-Kalman滤波，这个滤得不是很好。

X轴三种滤波对比图

放大图
一阶互补跟随快，但是抗干扰也会差一点，其他两个要等几个周期才会跟随比较好，应该是要更新参数。
上代码。

STM32F103VModel.zip (409.82 KB, 下载次数: 7197)

wszdxmh · 发表于 2016-7-22 15:51:00

只见下载，不见顶贴。。。

wszdxmh · 发表于 2016-7-22 16:49:10

本帖最后由 wszdxmh 于 2016-7-22 16:53 编辑

达克罗德发表于 2016-7-22 16:31
有没有输出欧拉角比较准确不漂移的算法
还有线性位移
这些算法才是真正有用的算法

MPU6050模块8块5一个，你还想怎么样？

我同学在质检局工作，人家的陀螺仪准到不行。我现在知道的也就只有这几种算法，跟我一起坐等大神回复吧。

wszdxmh · 发表于 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
目测你是移植了一个别人的错误的代码，不行就下载我的程序试试，还有，没有波形图吗？有什么比图更直接？

wszdxmh · 发表于 2016-7-23 22:27:02

橘子先生发表于 2016-7-23 21:54
问一下楼主u16类型的加速度计值和陀螺仪值在代入互补滤波的计算公式之前需要处理吗？如果需要那要怎么处理 ...

需要用反切计算为角度，因为输入的参数为速度和角度，所以要把采集的角速度转换成角度。你可以下载我的工程看看，里面都有。

wszdxmh · 发表于 2016-7-25 14:54:32

橘子先生发表于 2016-7-25 14:36
嗯，当加速度是1g的时候输出值是16384对吧？但是有点不明白你程序中加速度处理中
if (Accel_x

谢谢指正！应该是我打错了，我当时没注意。不过也能跑。。。

达克罗德 · 发表于 2016-7-22 16:31:16

有没有输出欧拉角比较准确不漂移的算法
还有线性位移
这些算法才是真正有用的算法

迷恋烟花的瞬间 · 发表于 2016-7-23 18:55:36

顶顶顶，学习一下

望着你发槑 · 发表于 2016-7-23 21:45:12

谢谢

橘子先生 · 发表于 2016-7-23 21:54:26

问一下楼主u16类型的加速度计值和陀螺仪值在代入互补滤波的计算公式之前需要处理吗？如果需要那要怎么处理？

橘子先生 · 发表于 2016-7-24 20:00:12

wszdxmh 发表于 2016-7-23 22:27
需要用反切计算为角度，因为输入的参数为速度和角度，所以要把采集的角速度转换成角度。你可以下载我的工 ...

啊，好的，我回去试试，谢啦

神秘狼 · 发表于 2016-7-24 21:05:37

顶顶顶

橘子先生 · 发表于 2016-7-25 13:40:41

问下楼主这里的分辨率是什么意思？跟实际测得的数值有什么联系?

wszdxmh · 发表于 2016-7-25 14:10:15

橘子先生发表于 2016-7-25 13:40
问下楼主这里的分辨率是什么意思？跟实际测得的数值有什么联系?

上面写得很清楚啊。
这个跟后面的算正负号有关系。

橘子先生 · 发表于 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是什么还是看不懂

橘子先生 · 发表于 2016-7-25 14:56:44

本帖最后由橘子先生于 2016-7-25 14:57 编辑

wszdxmh 发表于 2016-7-25 14:54
谢谢指正！应该是我打错了，我当时没注意。不过也能跑。。。

那那个49152是什么？而且我看好多人写的程序都是32764不是32768

Crystal子午123 · 发表于 2016-7-25 15:59:26

顶顶顶

wszdxmh · 发表于 2016-7-25 16:40:48

橘子先生发表于 2016-7-25 14:56
那那个49152是什么？而且我看好多人写的程序都是32764不是32768

应该是32768，因为是65535/2，他们写成那样我也不知道是为什么。

橘子先生 · 发表于 2016-7-25 18:02:36

wszdxmh 发表于 2016-7-25 16:40
应该是32768，因为是65535/2，他们写成那样我也不知道是为什么。

哦，好吧。。

wszdxmh · 发表于 2016-7-25 19:22:41

橘子先生发表于 2016-7-25 18:02
哦，好吧。。

我很怀疑是第一个共享代码的人写错了，后面的人都错了。

橘子先生 · 发表于 2016-7-25 22:03:18

wszdxmh 发表于 2016-7-25 19:22
我很怀疑是第一个共享代码的人写错了，后面的人都错了。

哈哈，有可能

王轩 · 发表于 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，其他的三个值正常！

wszdxmh · 发表于 2016-7-26 23:39:33

王轩发表于 2016-7-26 21:43
楼主您好~我移植的您的代码，可是经过卡尔曼计算出来之后的值，为什么一直都是-1呢？哪里出错了？
mpu6 ...

这个我就不知道你移植哪里出错了。我没遇过这个情况。

chuck__chee · 发表于 2016-7-27 08:50:42

为什么用了DMP 还用卡尔曼来算啊卡尔曼出来的也是欧拉角？看你的图片仅仅是对原始数据的滤波啊，但是如果直接用DMP，是不能对原始数据进行任何处理的啊，直接FIFO读数据了~

wszdxmh · 发表于 2016-7-27 09:19:28

chuck__chee 发表于 2016-7-27 08:50
为什么用了DMP 还用卡尔曼来算啊卡尔曼出来的也是欧拉角？看你的图片仅仅是对原始数据的滤波啊，但是 ...

你去看看那些开源四轴的代码，基本上都是自己滤波，有些第一代用DMP，第二代更新了之后也用改成自己滤波。至于具体的，我也没研究过，但是大家一致选择这种方案，就一定有他的道理。

王轩 · 发表于 2016-7-27 10:43:15

wszdxmh 发表于 2016-7-26 23:39
这个我就不知道你移植哪里出错了。我没遇过这个情况。

好吧~我再挣扎挣扎

jim_ · 发表于 2016-7-27 15:56:08

顶顶，学习学习学习学习.....

czdspeed · 发表于 2016-7-27 16:25:59

wszdxmh 发表于 2016-7-22 15:51
只见下载，不见顶贴。。。

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

wszdxmh · 发表于 2016-7-27 17:09:10

czdspeed 发表于 2016-7-27 16:25
楼主你研究pid是哪一个方面的，我这边也会抽时间研究，不过我们项目是研究pid参数的自整定不知道你有没有 ...

我现在只是在做静态的，自整定还没研究。到时底做到了这部分希望可以交流一下。

qq942266575 · 发表于 2016-7-27 18:39:58

顶一下。

王轩 · 发表于 2016-7-27 19:06:28

wszdxmh 发表于 2016-7-26 23:39
这个我就不知道你移植哪里出错了。我没遇过这个情况。

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

wszdxmh · 发表于 2016-7-27 19:51:17

王轩发表于 2016-7-27 19:06
突然之间发现了~起始代码都正常的，知识我用的stc单片机，要等2分多种才可以！波形才完全正常~谢谢啦！
...

我是直接用的。

王轩 · 发表于 2016-7-27 19:54:13

wszdxmh 发表于 2016-7-27 19:51
我是直接用的。

嗯谢谢啦~

电子之旅 · 发表于 2016-7-27 20:33:39

666，谢谢分享

swat0716 · 发表于 2016-7-28 09:17:20

最近正在研究卡尔曼滤波，谢谢分享。

2997553568 · 发表于 2016-7-28 09:46:07

可以加还好友交流吗？2997553568

还是看不穿 · 发表于 2016-7-28 10:39:40

最近正在学习这个。顶一下！！！！

显示全部楼层 · 发表于 2016-8-1 11:06:56

提示: 作者被禁止或删除内容自动屏蔽

科科1987 · 发表于 2016-8-1 11:51:27

好贴，谢谢楼主分享，收藏了。

来一颗糖 · 发表于 2016-8-1 13:14:54

顶一个，楼主偶看啦

long2016 · 发表于 2016-8-5 23:27:40

因为使用DMP速度太慢了，用DMP最大速率才200HZ

方向很重要 · 发表于 2016-8-6 01:22:32

楼主大神，请问你的源码是直接将GY-521对应连接到PA0与PA1上，之后串口1（PA9,PA10）直接发送给上位机是么？本人初学，请谅解一些很愚笨的问题，我按照我所说的连了线了，之后也下载进去了，我试了几次发现在while（1）之前的那while就卡住了，求解。谢谢您。

wszdxmh · 发表于 2016-8-6 10:06:41

方向很重要发表于 2016-8-6 01:22
楼主大神，请问你的源码是直接将GY-521对应连接到PA0与PA1上，之后串口1（PA9,PA10）直接发送给上位机是么 ...

如果是卡在DMP的初始化的话，请把模块放平再试一次，之前我就是这样的。

华丽与creazy · 发表于 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);    //卡尔曼滤波计算倾角

华丽与creazy · 发表于 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
目测你是移植了一个别人 ...

波形图？不知道怎么弄出波形图，软件吗？

华丽与creazy · 发表于 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或者微信什么的，求教

JueSe · 发表于 2016-8-10 14:00:59

顶

wszdxmh · 发表于 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
自己看吧，我在这个帖子里我有说一些使用方法。本贴也有共享代码和上位机软件，你真的没看到？

华丽与creazy · 发表于 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;
是因为这两行代码的原因吗？我们除的参数不同

华丽与creazy · 发表于 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（即角速度）

帐号		自动登录	找回密码
密码			立即注册

坤坤啦该用户已被删除	发表于 2016-8-1 11:06:56 \| 显示全部楼层提示: 作者被禁止或删除内容自动屏蔽
坤坤啦该用户已被删除
	回复支持反对使用道具举报

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

原子哥极力推荐 /2