查看: 5533|回复: 24
贡献一个mpu6050的卡尔曼滤波程序
本帖最后由 salahading 于
20:33 编辑
/**********Kalman_Filter *********/
/* 系统状态变量*/
float x[2] = {0,0};
/* 采样时间 */
float Ts = 0.001;
/*误差的协方差矩阵*/
float p[2][2] = {{1,0},{0,1}};
/* Kalman增益 */
float K[2] = {0,0};
/* 系统状态协方差矩阵对角元素 */
float Q0 = 0.00001,Q1 = 0.00003;
/* 系统输出 高斯噪声方差*/
float R = 0.5;
/*&&
& & & & Zk 倾角仪器或加速度计测量的系统角度&&Gyro 陀螺仪的角速度输出
& & & & x[0] 系统倾角
& & & & x[1] 陀螺仪漂移
*/
void Kalman_Filter(float Zk,float Gyro)& & & & & & & &
& & & & /*
&&& & & & X(k+1) = AX(k) + B*U
& & & && &A = {{1,-Ts},{0,1}}& & B = {Ts,0}
& & & && &Zk = Hx
& & & && &Zk&&系统输出& & H = {1 ,0}
& & & && &计算系统状态估计值
& & & & */
& & & & x[0] = x[0] + Ts*(Gyro - x[1]);
& & & & x[1] = x[1];
& & & & /*
& & & && &误差的协方差估计
& & & & */
& & & & p[0][0] = p[0][0] - Ts*(p[0][1]+p[1][0]) + Ts*Ts*p[1][1] + Q0;
& & & & p[0][1] = p[0][1] - Ts*p[1][1];
& & & & p[1][0] = p[1][0] - Ts*p[1][1];
& & & & p[1][1] = p[1][1] + Q1;
& & & &
& & & & /*
& & & & & & & & 计算Kalman增益
& & & & */
& & & & K[0] = p[0][0]/(p[0][0]+R);
& & & & K[1] = p[1][0]/(p[0][0]+R);
& & & &
& & & & /*
& & & & & & & & 校正先验协方差估计
& & & & */
& & & & p[0][0] = p[0][0] - K[0]*p[0][0];
& & & & p[0][1] = p[0][1] - K[0]*p[0][1];
& & & & p[1][0] = p[1][0] - K[1]*p[0][0];
& & & & p[1][1] = p[1][1] - K[1]*p[0][1];
& & & &
& & & & /*
& & 校正系统状态估计值
& & & & */
& & & & x[0] = x[0] + K[0]*(Zk - x[0]);
& & & & x[1] = x[1] + K[1]*(Zk - x[0]);
& & & &
有同学说的他人的程序是不是下面的
void Kalman_Filter1(float Accel,float Gyro)& & & & & & & &
{
& & & & Angle+=(Gyro - Q_bias) * //先验估计
& & & & Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
& & & & Pdot[1]=- PP[1][1];
& & & & Pdot[2]=- PP[1][1];
& & & & Pdot[3]=Q_
& & & &
& & & & PP[0][0] += Pdot[0] *& &// Pk-先验估计误差协方差微分的积分
& & & & PP[0][1] += Pdot[1] *& &// =先验估计误差协方差
& & & & PP[1][0] += Pdot[2] *
& & & & PP[1][1] += Pdot[3] *
& & & & & & & &
& & & & Angle_err = Accel - A& & & & //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_& & & &&&//后验估计
& & & & Q_bias& & & & += K_1 * Angle_& & & &&&//后验估计
& & & & Gyro_y& &= Gyro - Q_& & & &&&//输出值(后验估计)的微分=角速度
}复制代码
那这个是不是卡尔曼滤波呢？
收下了 你应该讲解两句的 什么入口出口的
你确定这是Kalman滤波？
&&Mark.谢谢楼主的分享！
这是什么卡尔曼滤波&&？
你确定这是Kalman滤波？
你确定不是吗
这是什么卡尔曼滤波&&？
离散卡尔曼
有人验证过么？？效果怎么样？
你确定不是吗
quite sure
像是Kalman filter的形式，但是估计的只是一个scaler而不会vector，没看懂在估计啥？
之前看过别人写的。你这个应该不是!
单值滤波，算不上卡尔曼，卡尔曼实际上是融合过程，不是滤波过程
单值滤波，算不上卡尔曼，卡尔曼实际上是融合过程，不是滤波过程
看一下第二段程序是不是？
之前看过别人写的。你这个应该不是!
你说的是不是第二段程序呢？
你确定这是Kalman滤波？
请问第二段程序是不是卡尔曼滤波呢？
你确定这是Kalman滤波？
还请指教啊
之前看过别人写的。你这个应该不是!
还请指教啊
第二段应该是，你可以假设一组数据然后加入高斯噪声，看看滤波后的效果
第二段应该是，你可以假设一组数据然后加入高斯噪声，看看滤波后的效果 ...
刚看完电影，心情不错，就不刻薄了，只说一句，别不懂装懂
刚看完电影，心情不错，就不刻薄了，只说一句，别不懂装懂
你的自信从何而来?
刚看完电影，心情不错，就不刻薄了，只说一句，别不懂装懂
第二段估计就是网上抄的一段mpu-6050程序
先收藏着，以后有时间研究下
你说的是不是第二段程序呢？
卡尔曼滤波算法C语言实现(转2)
(出处: amoBBS 阿莫电子论坛)
这个帖子讲得满详细，有研究的可以参考看看。
你说的是不是第二段程序呢？
卡尔曼滤波算法C语言实现(转2)
(出处: amoBBS 阿莫电子论坛)
这个帖子讲得满详细，有研究的可以参考看看。 论坛里发这个算法的人很多，你自己收看看，对比下。
别人纠正的不对，只是为你好，并不是想对你怎么样。
阿莫电子论坛, 原"中国电子开发网"MPU6050卡尔曼滤波代码_百度文库
两大类热门资源免费畅读
续费一年阅读会员，立省24元！
评价文档：
MPU6050卡尔曼滤波代码
上传于||暂无简介
大小：3.90KB
登录百度文库，专享文档复制特权，财富值每天免费拿！
你可能喜欢MPU6050数据分析与滤波_百度文库
两大类热门资源免费畅读
续费一年阅读会员，立省24元！
MPU6050数据分析与滤波
上传于||暂无简介
阅读已结束，如果下载本文需要使用1下载券
想免费下载本文？
定制HR最喜欢的简历
下载文档到电脑，查找使用更方便
还剩13页未读，继续阅读
定制HR最喜欢的简历
你可能喜欢MPU6050(初步调试代码：度数相差1-2度)
单片机&嵌入式
单片机应用
嵌入式操作系统
学习工具&教程
学习和开发单片机的必备工具
(有问必答)
(带你轻松入门)
电子元件&电路模块
当前位置： >>
>> 浏览文章
MPU6050(初步调试代码：度数相差1-2度)
&************************************************************************/
#define &YCC_GYRO_GLOBALS
#include &stm32f10x.h&
#include &main.h&
#include &stdio.h&
#include &math.h&
#include &string.h&
//定义MPU6050内部地址********************
#define SMPLRT_DV & &0x19 //采样率分频，典型值:0x07(125Hz)
#defineCONFIG0x1A //低通滤波频率，典型值:0x06(5Hz)
#define GYRO_CONFIG0x1B //陀螺仪自检及测量范围及高通滤波频率，典型值:0x18(不自检，2000deg/s)
#define ACCEL_CONFIG0x1C //加速计自检、测量范围及高通滤波频率，典型值:0x01(不自检,2G,5Hz)
#define AX_H0x3B //储存最近的X,Y,Z轴加速度计测量值
#defineAX_L0x3C
#defineAY_H0x3D
#defineAY_L0x3E
#defineAZ_H0x3F
#defineAZ_L0x40
#defineTEMP_H0x41 //储存最近温度传感器的测量值
#define TEMP_L0x42
#define GX_H0x43 //储存最近的X,Y,Z轴陀螺仪感应器的测量值
#define GX_L0x44
#define GY_H0x45
#define GY_L0x46
#define GZ_H0x47
#define GZ_L0x48
#define PWR_M0x6B //电源管理，典型值:0x00(正常启动)
#define FIFO_COUNT_H0x72 //必须按从高到低的顺序读取
#define FIFO_COUNT_L0x73
#define WHO_AM_I0x75 //IIC地址寄存器(默认数值:0x68,只读)
#defineMPU6050_Addr & &0xD0 &//定义器件在IIC总线中的从地址,根据ALT &ADDRESS地址引脚不同修改
//******角度参数************
static float Angle_x,Angle_y,Angle_z; &//小车最终倾斜角度
static float Accel_x,Accel_y,Accel_z; &//小车加速度计算角度&
//****************************
//加速度轴和陀螺仪轴的零点漂移，取4000个数据的平均值
#define Ax_offset 0x03D2
#define Ay_offset0xFED4
#define Az_offset0x5956
#define Gx_offset0x0011
#define Gy_offset0xFFE3
#define Gz_offset0xFFF5
#define PI3.14
#define gyro_time0.01 &//s
//******卡尔曼参数************ & & & & & & & &
float Q_angle=0.001; &
float Q_gyro=0.003;
float R_angle=0.5;
float dt=0.01; & & & & & & & & & & & & &//dt为kalman滤波器采样时间;
float C_0 = 1.0;
float Q_bias, Angle_
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 } };
//*********************************************************
typedef struct
}GYRO_INFO;
static GYRO_INFO gyro_
static void Single_Write(u8 register_add,u8 date)
I2C_Bus(I2C_DEV_MPU3050,I2CMD_IOTODEV,register_add,&date,1);
static s16 Single_Read(u8 register_add)
u8 date[2];
I2C_Bus(I2C_DEV_MPU3050,I2CMD_DEVTOIO,register_add,date,2);
temp= (date[0]&&8)|date[1];
return (s16)
void Gyro_Init()
& &Single_Write(PWR_M, 0x00); & //重置所有寄存器，温度传感器失能
& &Single_Write(SMPLRT_DV,0x07);//陀螺仪&加速度计采样率，典型值：0x07(125Hz)
& &Single_Write(CONFIG,0x06); & //低通滤波频率，典型值：0x06(5Hz)
& &Single_Write(GYRO_CONFIG,0x08);//(+-500deg/s
// & Single_Write(GYRO_CONFIG,0x18);//陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
& &Single_Write(ACCEL_CONFIG,0x01);//加速度计(不自检，2g)
//******读取MPU3050数据****************************************
static u8 READ_MPU3050()
u8 power=0x40,
s16 temp[3];
I2C_Bus(I2C_DEV_MPU3050,I2CMD_DEVTOIO,PWR_M,&power,1);
& &if(power != 0x40)
& & & temp[0] = Single_Read(AX_H);
& temp[1] = Single_Read(AY_H);
& temp[2] = Single_Read(AZ_H);
& & & &gyro_info.ax=(s16)(temp[0])/16384.00; //范围为2g时，换算关系：16384 LSB/g
& gyro_info.ay=(s16)(temp[1])/16384.00; //角度较小时，x=sinx得到角度（弧度）, deg = rad*180/3.14
& gyro_info.az=(s16)(temp[2])/16384.00; //因为x&=sinx,故乘以1.3适当放大
& gyro_info.gx=(s16)(Single_Read(GX_H)-Gx_offset)/65.5;///16.4;&
& gyro_info.gy=(s16)(Single_Read(GY_H)-Gy_offset)/65.5;///16.4;
& gyro_info.gz=(s16)(Single_Read(GZ_H)-Gz_offset)/65.5;///16.4;
/* & Accel_x = atan(gyro_info.ay/gyro_info.az)*180/PI;
& Accel_y = atan(gyro_info.ax/gyro_info.az)*180/PI;*/
& Accel_x =asin((s16)(temp[0]-Ax_offset)/0/PI;
& Accel_y =asin((s16)(temp[1]-Ay_offset)/0/PI;
& Accel_z =asin((s16)(temp[2]-Az_offset)/0/PI;
& &Gyro_Init();
/*********************************************************/
/*************倾角计算(卡尔曼滤波融合)********************/
/*********************************************************/
//Kalman滤波，20MHz的处理时间约0.77ms；
void Kalman_Filter(float Accel,float* Gyro,float* outAngle)//,float*outAngleDot)
//static float Angle,AngleD
*outAngle+=(*Gyro - Q_bias) * //先验估计
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
Pdot[1]=- PP[1][1];
Pdot[2]=- PP[1][1];
Pdot[3]=Q_
PP[0][0] += Pdot[0] * & // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * & // =先验估计误差协方差
PP[1][0] += Pdot[2] *
PP[1][1] += Pdot[3] *
Angle_err = Accel -*outA//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;
*outAngle+= K_0 * Angle_ //后验估计
Q_bias+= K_1 * Angle_ //后验估计
*Gyro = *Gyro - Q_ //输出值(后验估计)的微分=角速度
//*outAngle=*outA
//*outAngleDot=AngleD
/******************互补滤波**********************************
*补偿原理是取当前倾角和加速度获得倾角差值进行放大，然后与
* 陀螺仪角速度叠加后再积分，从而使倾角最跟踪为加速度获得的角度
* 0.5为放大倍数，可调节补偿度；gyro_time为系统周期10ms & & & &&
**************************************************************/
&void Angle_Calcu(void)&
READ_MPU3050();
Kalman_Filter(Accel_y,&gyro_info.gx,&Angle_x);
& & Kalman_Filter(Accel_x,&gyro_info.gy,&Angle_y);
//Kalman_Filter(Angle_z,Accel_z,gyro_info.gz);
//Angle_x = Angle_x +(((Accel_y-Angle_x)*0.5 + gyro_info.gx)*gyro_time);
//Angle_y = Angle_y +(((Accel_x-Angle_y)*0.5 + gyro_info.gy)*gyro_time);
//Angle_z = Angle_z +(((Accel_x-Angle_y)*0.5 + gyro_info.gy)*gyro_time);
#ifdef DEBUG_MODE
void Gyro_Report()
u8 i,len,temp[50];
char str1[2]={'a','g'};
char str2[3]={'x','y','z'};
float Angle[3];
p=(float*)(&gyro_info);
//p=(s16*)(&gyro_offset);
Angle[0]=Angle_x;
Angle[1]=Angle_y;
Angle[2]=Angle_z;
if(I2C_Probe(I2C_DEV_MPU3050)==1)
for(i=0;i&6;i++)
if(i&3) &len=sprintf((char*)temp,&%c%c:%.2f&,str1[0],str2[i],*p++);
else & & len=sprintf((char*)temp,&%c%c:%.2f&,str1[1],str2[i-3],*p++);
Host_Report_Event(HOST_EVENT_DEBUG,temp,len);
for(i=0;i&3;i++)
len=sprintf((char*)temp,&Angle%c:%.2f&,str2[i],Angle[i]);
Host_Report_Event(HOST_EVENT_DEBUG,temp,len);
void Gyro_Main()
Angle_Calcu();
【】【】【】【】
上一篇:下一篇:
CopyRight @
单片机教程网
, All Rights ReservedMPU6050的数据处理疑问_stm32吧_百度贴吧
&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&签到排名：今日本吧第个签到，本吧因你更精彩，明天继续来努力！
本吧签到人数：0成为超级会员，使用一键签到本月漏签0次！成为超级会员，赠送8张补签卡连续签到：天&&累计签到：天超级会员单次开通12个月以上，赠送连续签到卡3张
关注：18,950贴子：
MPU6050的数据处理疑问收藏
菜鸟我最近在调试这个6050，我先把程序的一些设置给大家说下，大家帮我看看吧~！拜托了陀螺仪：灵敏度 16.4 LSB/(°/s)
量程+/- 2000°/s加速度仪：灵敏度 16384 LSB/g 量程+/- 2g问题来了，我先把6050放在桌面上，采集回来的数据如图：上图中大家看AZ=0.9g还能理解 因为重力加速度在可是当我把6050片子以Y轴进行180度来个底朝天旋转，大家请看图：为什么AY达到了3.8个g呢？而AZ也到了2.9个g？但是我选的量程才+/-2g啊？这是什么情况啊
三叔正版授权网游公测！
坐等大神前来支招
mpu6050的输出是16位补码形式的，第一位是符号位，0便是正，1是负，从0000到ffff，其中0000-7fff是表示正的0-2g，8000-ffff为负的0-2g你是把得到的值直接除了Sensitivity得到的吧
楼主，你好，我最近正在学用STM32写MPU6050，可是进展不是很好，希望能有一份好的例程学习，可以把你的完整的源程序发给我吗？我的邮箱
楼主 求你的程序
我读出来的数据更坑 拯救一下我 行不
LZ那个量程是怎么设定的啊。。。
楼主，请问原始数据应该乘以灵敏度还是除以灵敏度才得到真正的加速度或者角速度啊？
请问mpu6050得到的数据通过模拟i2c之后怎么通过串口输出？这些数据具体是怎样的？
楼主！能给一份代码吗？我到现在还没测出值来！一直显示0，。。。
和张大佛爷、二月红一起去探秘矿洞墓穴！
,同求、楼主好人，万分感谢
楼主，我觉得你帖子发错了，一发一大把人问你问题
我集中回复下吧 好久不做这个了 生疏了 大家沉了吧
网上例程一大把
楼上的说符号问题，应该就是了。你连续读了6个u8的数据，两个一组，成了3个数据，假设是data[8]，那第一个数据应该是(short)(((u16)data[0]&&8) | data[1])这个数就是带符号的了，剩下的数据一样处理
楼主问题解决了么？我也是一样的问题，感觉也是补码的问题，能说下怎么解决的么？
没想到这个帖子到现在还是有人回复，真是谢谢大家了~~~~~我这个问题已经弄好了，不过，好久没做过这个方面了，自己也不记得如何处理的了~~~~
哈哈 我来回复下 这个问题我也是遇到，是楼上说的符号位的情况，具体怎么做我也没有找到有效的方法，这样，判断最高位是否为1 为1代表负数，拿最大数65536减去当前的值就是另一方向的加速度
登录百度帐号推荐应用
为兴趣而生，贴吧更懂你。或