查看: 639|回复: 3
mpu6050用卡尔曼滤波算出角度，不知道哪里错了，输出角度是0，
主题帖子精华
初级会员, 积分 132, 距离下一级还需 68 积分
在线时间0 小时
#include "sys.h"
#include "usart.h"
#include "delay.h"
#include "led.h" &&
#include "lcd.h" &
#include "key.h" &&
#include "mpu6050.h"&
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "math.h"
#define pi 3.
//函数声明
void &mpu6050_dmp(void);//MPU6050状态量输出
void Kalman_Filter(float Gyro,float Accel);
//函数变量
float Angle=0,Gyro_x=0; & & & & //小车滤波后倾斜角度/角速度
//******卡尔曼参数************
float &Q_angle=0.001; &
float &Q_gyro=0.003;
float &R_angle=0.5;
float &dt=0.005;
& & & & & & & & &//dt为kalman滤波器采样时间;
char & C_0 = 1;
float &Q_bias=0, Angle_err=0;
float &nbspCt_0=0, PCt_1=0, E=0;
float &K_0=0, K_1=0, t_0=0, t_1=0;
float &nbspdot[4] ={0,0,0,0};
float &nbspP[2][2] = { { 1, 0 },{ 0, 1 } };
//全局变量
float pitch,roll,
short aacx,aacy,
//加速度传感器原始数据
short gyrox,gyroy, //陀螺仪原始数据
& & //温度
float Ax,Ay,Az;//单位 g(9.8m/s^2)
float Gx,Gy,Gz;//单位 °/s
& float Angle_accX,Angle_accY,Angle_accZ;//存储加速度计算出的角度
& float Angle_gyroX,Angle_gyroY,Angle_gyroZ;//存储角速度计算出的角度
&int main(void)
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
delay_init();
//延时函数初始化
uart_init(9600);
//串口初始化为9600
LED_Init();
//初始化与LED连接的硬件接口
KEY_Init();
//初始化按键
LCD_Init();
//初始化LCD &
MPU_Init();
//初始化MPU6050
while(mpu_dmp_init())//监测模块是否正常；
LCD_ShowString(30,130,200,16,16,"MPU6050 Error");
delay_ms(500);
POINT_COLOR=BLUE;//设置字体为蓝色&
& LCD_ShowString(30,80,200,16,16," aacx: & &. C");
& LCD_ShowString(30,100,200,16,16,"gyrox: & &. C");
& LCD_ShowString(30,120,200,16,16," aacy: & &. C"); &
& LCD_ShowString(30,140,200,16,16," gyroy : &. C");
LCD_ShowString(30,160,200,16,16," aacz: & &. C");
& LCD_ShowString(30,180,200,16,16,"gyroz: & &. C");
& LCD_ShowString(30,200,200,16,16," Temp: & &. C");
& LCD_ShowString(30,220,200,16,16,"Pitch: & &. C");
& LCD_ShowString(30,240,200,16,16," Roll: & &. C"); &
& LCD_ShowString(30,260,200,16,16," Yaw : & &. C"); &
& while(1)
//======一下三行是对加速度进行量化，得出单位为g的加速度值-2g量程
Ax=aacx/16384.00;
& &Ay=aacy/16384.00;
& &Az=aacz/16384.00;
& &//==========以下三行是用加速度计算三个轴和水平面坐标系之间的夹角
& &Angle_accY= atan(Ax / Az)*180/ & & //加速度仪，反正切获得弧度值，乘以180度/pi&
& &Angle_accX= atan(Ay / Az)*180/ & //获得角度值，乘以-1得正
& &//==========以下三行是对角速度做量化-2000°量程==========
& &Gx=gyrox/16.38;
& &Gy=gyroy/16.38;
& &Gz=gyroz/16.38;
&Kalman_Filter(Gy,Angle_accX);
mpu6050_dmp();
& if(Angle_accX&0){
LCD_ShowChar(30+48,20,'-',16,0);
& & //显示负号
Angle_accX=-Angle_accX; }
& & & & & & & //转为正数
else LCD_ShowChar(30+48,20,' ',16,0);
//去掉负号&
LCD_ShowNum(30+48+8,20,Angle_accX,5,16);
//显示整数部分
&LCD_ShowChar(30+48,40,'-',16,0);
& & //显示负号
& & & & & & //转为正数
else LCD_ShowChar(30+48,40,' ',16,0);
//去掉负号
LCD_ShowNum(30+48+8,40,Gx,5,16);
//显示整数部分
if(Angle&0){
&LCD_ShowChar(30+48,60,'-',16,0);
& & //显示负号
& & & & & & //转为正数
else LCD_ShowChar(30+48,60,' ',16,0);
//去掉负号
LCD_ShowNum(30+48+8,60,Angle,5,16);
//显示整数部分
void &mpu6050_dmp(void)
if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)
temp=MPU_Get_Temperature(); //得到温度值
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //得到加速度传感器数据
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺仪数据
if(temp&0)
LCD_ShowChar(30+48,200,'-',16,0);
//显示负号
//转为正数
LCD_ShowChar(30+48,200,' ',16,0);
//去掉负号&
LCD_ShowNum(30+48+8,200,temp/100,3,16);
//显示整数部分
LCD_ShowNum(30+48+40,200,temp%10,1,16);
//显示小数部分
temp=pitch*10;
if(temp&0)
LCD_ShowChar(30+48,220,'-',16,0);
//显示负号
//转为正数
LCD_ShowChar(30+48,220,' ',16,0);
//去掉负号&
LCD_ShowNum(30+48+8,220,temp/10,3,16);
//显示整数部分
LCD_ShowNum(30+48+40,220,temp%10,1,16);
//显示小数部分&
temp=roll*10;
if(temp&0)
LCD_ShowChar(30+48,240,'-',16,0);
//显示负号
//转为正数
LCD_ShowChar(30+48,240,' ',16,0);
//去掉负号&
LCD_ShowNum(30+48+8,240,temp/10,3,16);
//显示整数部分
LCD_ShowNum(30+48+40,240,temp%10,1,16);
//显示小数部分&
temp=yaw*10;
if(temp&0)
LCD_ShowChar(30+48,260,'-',16,0);
//显示负号
//转为正数
LCD_ShowChar(30+48,260,' ',16,0);
//去掉负号&
LCD_ShowNum(30+48+8,260,temp/10,3,16);
//显示整数部分
LCD_ShowNum(30+48+40,260,temp%10,1,16);
//显示小数部分&
& & & & temp=aacx*10;
if(temp&0)
LCD_ShowChar(30+48,80,'-',16,0);
//显示负号
//转为正数
LCD_ShowChar(30+48,80,' ',16,0);
//去掉负号&
LCD_ShowNum(30+48+8,80,temp/10,5,16);
//显示整数部分
& & & & temp=gyrox*10;
if(temp&0)
LCD_ShowChar(30+48,100,'-',16,0);
//显示负号
//转为正数
LCD_ShowChar(30+48,100,' ',16,0);
//去掉负号&
LCD_ShowNum(30+48+8,100,temp/10,5,16);
//显示整数部分
&temp=aacy*10;
if(temp&0)
LCD_ShowChar(30+48,120,'-',16,0);
//显示负号
//转为正数
LCD_ShowChar(30+48,120,' ',16,0);
//去掉负号&
LCD_ShowNum(30+48+8,120,temp/10,5,16);
//显示整数部分
& & & & temp=gyroy*10;
if(temp&0)
LCD_ShowChar(30+48,140,'-',16,0);
//显示负号
//转为正数
LCD_ShowChar(30+48,140,' ',16,0);
//去掉负号&
LCD_ShowNum(30+48+8,140,temp/10,5,16);
//显示整数部分
temp=aacz*10;
if(temp&0)
LCD_ShowChar(30+48,160,'-',16,0);
//显示负号
//转为正数
LCD_ShowChar(30+48,160,' ',16,0);
//去掉负号&
LCD_ShowNum(30+48+8,160,temp/10,5,16);
//显示整数部分
& & & & temp=gyroz*10;
if(temp&0)
LCD_ShowChar(30+48,180,'-',16,0);
//显示负号
//转为正数
LCD_ShowChar(30+48,180,' ',16,0);
//去掉负号&
LCD_ShowNum(30+48+8,180,temp/10,5,16);
//显示整数部分
void Kalman_Filter(float Gyro,float Accel)
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
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_x & = Gyro - Q_
//输出值(后验估计)的微分=角速度
主题帖子精华
在线时间159 小时
帮顶。。。。
我的淘宝小店：
主题帖子精华
新手上路, 积分 3, 距离下一级还需 47 积分
在线时间0 小时
发表于 昨天&17:37
我也遇到相同的问题，请问楼主解决了么？求教
主题帖子精华
在线时间159 小时
发表于 昨天&20:33
Powered by三轴陀螺仪mpu6050测试程序_图文_百度文库
两大类热门资源免费畅读
续费一年阅读会员，立省24元！
三轴陀螺仪mpu6050测试程序
上传于||文档简介
&&I​n​v​e​n​S​e​n​s​e​公​司​的​三​轴​陀​螺​仪​M​P​U00​测​试​程​序​。​I​I​C​接​口​,1​单​片​机​驱​动​,​L​C​D62​同​步​显​示​。
阅读已结束，如果下载本文需要使用1下载券
想免费下载本文？
下载文档到电脑，查找使用更方便
还剩6页未读，继续阅读
你可能喜欢}