新人求教 MPU6050 加速度读出后怎么滤波啊
本人小白一个MPU6050 加速度读出来之后 减去零漂之后 加速度的值一直在0处上下浮动（这个是不是因为我的MPU6050坏了？？这现象正常么？？） 然后用叉积法（网上看的博士的程序后面注释的）融合陀螺和加速度 然后结果就是一只乱飘
是不是因为我的加速度读出来没滤波，求教各位大神我该用什么方法滤波啊？？
论坛里有现成的代码，你搜搜吧 mahengyu 发表于 2013-11-5 21:17 static/image/common/back.gif
论坛里有现成的代码，你搜搜吧
嗯，谢谢了
我还有个疑问 就是MPU6050用不用对它进行设置什么的来修正这个问题？？这个数据手册 中文的好像没说（我觉得他不全），英文的看不懂···· mjkxxn 发表于 2013-11-5 21:27 static/image/common/back.gif
嗯，谢谢了
我还有个疑问 就是MPU6050用不用对它进行设置什么的来修正这个问题？？这个数据手册 中文的 ...
不用
6050有dmp，直接输出结算后的四元数，你可以试试 自带dmp的 mahengyu 发表于 2013-11-5 21:49 static/image/common/back.gif
不用
6050有dmp，直接输出结算后的四元数，你可以试试
嗯我去找找DMP
论坛里有现成的代码 最近我也在学习卡尔曼滤波！从网上移植了个卡尔曼滤波，结果可以很好的跟踪角度，目前只能实现单个角度融合哈哈
所以今天和大家一起分享一下源码
/* IMU.c file
编写者：chushibin
网址：xinjiangdaxue
作者E-mail：419500541@qq.com
编译环境：MDK-LiteVersion: 4.23
初版时间: 2014-0-13
测试： 本程序已在第七实验室的上完成测试
难点：（1）实现卡尔曼滤波
（2）实现在动态下准确的测量姿态
占用STM32 资源：
1. 使用Tim7定时器 产生us级的系统时间
功能：
姿态解算 IMU
将传感器的输出值进行姿态解算。得到目标载体的俯仰角和横滚角 和航向角
个人学习：（1）陀螺仪输出的单位是弧度，需要转换成度每秒
（2）
------------------------------------
*/
/************************************************
***************卡尔曼滤波程序********************
*************************************************
*/
#include "IMU.h"
volatile uint32_t lastUpdate, now;
volatile uint16_t sysytem_time_ms=0;
volatile floatIMU_GYROx, IMU_GYROy, IMU_GYROz;
volatile float IMU_Pitch, IMU_Roll, IMU_Yaw;
volatile float incAngle;
volatile float g=-9.8f;
volatile float mygetzhi;
volatile float gyrox,gyroy,gyroz;
static float P = {{ 1, 0 },
{ 0, 1 }};//预测方差矩阵
float angle; //下一时刻角度的估计值
float q_bias=0; //陀螺仪的偏差，即角度的测量误差
float rate; //修正后的陀螺仪测量量即角速度
//R=
static const float R_angle = 0.001;//R代表观测协方差矩阵，
//这里我们估计的单一的角度
//Q=,
// 过程协方差
static const float Q_angle = 0.001;
static const float Q_gyro= 0.0015;
//A=,
//;
void IMU_init(void)
{
MPU6050_initialize();
HMC5883L_SetUp();
MS561101BA_init();
delay_ms(50);
MPU6050_initialize();
HMC5883L_SetUp();
lastUpdate = micros();//更新时间
now = micros();
}
/**************************实现函数********************************************
*函数原型: void IMU_getValues(float * values)
*功 能: 读取加速度 陀螺仪 磁力计 的当前值
输入参数： 将结果存放的数组首地址
输出参数：没有
*******************************************************************************/
#define new_weight 0.35f
#define old_weight 0.65f
void IMU_getValues(float * values)
{
int16_t accgyroval;
staticfloat lastacc= {0,0,0};
int i;
//读取加速度和陀螺仪的当前ADC
//accgyroval[]所对应实际存储的内容是ax/ay/az/（加速度计的测量值）, gx/gy/gz(陀螺仪的测量值)
MPU6050_getMotion6(&accgyroval, &accgyroval, &accgyroval, &accgyroval, &accgyroval, &accgyroval);
for(i = 0; i<6; i++)
{
if(i < 3)
{ //当前的加速度计当前测量值*0.35+上次加速度计的测量值*0.65=当前时刻加速度计的值
//其意义在于：当飞机没有起飞的时候，此时的飞机处于静止状态，测得的加速度计的值最为准确
//为了保证飞机在起飞后加速度的偏离的不那么块，所以前一次的测量值我们更相信一些
//也就是说更相信上一次的测量值
values = (float) accgyroval * new_weight +lastacc * old_weight ;
lastacc = values;
}
else
{
//将陀螺仪的测量值换算成度每秒
//根据陀螺仪编程手册我们可以看到，量程在2000度每秒时，陀螺仪的测量值应当除以16.4
//不同的量程对应不同的分辨率
values = ((float) accgyroval) / 16.4f; //转成度每秒
//这里已经将量程改成了 2000度每秒16.4 对应 1度每秒
}
}
HMC58X3_mgetValues(&values); //读取磁力计的ADC值
//values是首地址，也就是说磁强计的测量值被存储在了values的6、7、8中
//以下对应的是陀螺仪的测量值
//注意此时的测量值时弧度每秒
IMU_GYROx = accgyroval;
IMU_GYROy = accgyroval;
IMU_GYROz = accgyroval;
//（横滚）卡尔曼滤波直接是对角度误差滤波
//这里需要将从陀螺仪读出的弧度每秒的单位转换成度每秒
gyrox=values*180/M_PI;
gyroy=values*180/M_PI;
gyroz=values*180/M_PI;
}
/**********实现函数************************************
函数原型：void stateUpdate(const float q_m)
功能 ：更新状态并计算陀螺仪测量值得到的角度
状态更新，主要是计算陀螺仪的测量得到的角度
同时还更新误差方程矩阵P
输入 ：当前陀螺仪的测量值（已经转换成弧度）
*/
void stateUpdate(const float q_m)
{
volatile float halfT;
float q;//修正的角速度
float Pdot; //
now = micros();//读取时间
if(now < lastUpdate)
{ //定时器溢出过了。
halfT =((float)(now + (0xffffffff- lastUpdate)) / 2000000.0f);
lastUpdate = now;
//return ;
}
else
{
halfT =((float)(now - lastUpdate) / 2000000.0f);
}
lastUpdate = now; //更新时间
q = q_m - q_bias;//修正的角速度=当前陀螺仪测量值-估计的偏差
//将协方差做局部线性化，因此可以继续使用标准的卡尔曼滤波
//对协方差求导进行线近似。Pdot=A*P+p*AT+Q,此处的AT是A的转置举证
//以下计算就是根据Pdot=A*P+p*AT+Q展开计算而得
Pdot = Q_angle - P - P;
Pdot = - P;
Pdot = - P;
Pdot = Q_gyro;
rate = q;//存储修正后的角速度
angle += q *2*halfT ;//计算由陀螺仪得到的角度
//更新协方差矩阵
P += Pdot * 2*halfT;
P += Pdot * 2*halfT;
P += Pdot * 2*halfT;
P += Pdot * 2*halfT;
}
/**********函数实现****************************
函数原型：void incAngleUpdate(float values )
功能 ：计算加速度计和磁强计测量得到的角度
输入参数：values即加速度计的测量值
输出 ：无
个人说明：利用加速度计计算得到俯仰角和横滚角，在已知俯仰和横滚角度时
利用磁强计计算可得航向角
*/
void incAngleUpdate(float *shuzhi)
{
float accpitch,accroll;
float accyaw;
float acc;
float N,M;
//加速度计在+-2G的范围内工作，其分辨率为16384.0f
acc=shuzhi/16384.0f;
acc=shuzhi/16384.0f;
acc=shuzhi/16384.0f;
if(acc>1.0) acc=1.0;
if(acc<-1.0) acc=-1.0;
if(acc>1.0) acc=1.0;
if(acc<-1.0) acc=-1.0;
if(acc>1.0) acc=1.0;
if(acc<-1.0) acc=-1.0;
accpitch=asin(acc)*180/M_PI;
//将加速度计计算得到的角度转换成度单位，所以需要乘以180/M_PI
accroll=-asin(acc/cos(accpitch))*180/M_PI;
//N和M分别是计算航向的分子和分母
//其中N=my*cos(accroll)+mz*sin(accroll)
//其中M=mx*cos(accpitch)+my*sin(accpitch)*sin(accroll)-mz*cos(accpitch)*sin(accroll)
N=shuzhi*cos(accroll)+shuzhi*sin(accroll);
M=shuzhi*cos(accpitch)+shuzhi*sin(accpitch)*sin(accroll)-shuzhi*cos(accpitch)*sin(accroll);
accyaw=N/M;
incAngle=accroll;
incAngle=accpitch;
incAngle=accyaw;
}
/**********函数实现*************************
函数原型：void kalmanUpdate( float accAngle)
功能 ：进行卡尔曼滤波，得出最优估计
输入 ：加速计测量得到的角度incAngle
输出 ：无
*/
void kalmanUpdate(float *accAngle)
{
float angle_m = accAngle;//加速度计得到的角度
float angle_err = angle_m - angle;//角度误差=加速度计得到的角度-陀螺仪得到的角度
//H=
float h_0=1.0;
float h_1=0.0;
const float PHt_0 = h_0*P;
const float PHt_1 = h_0*P;
//E=HPH'+R
float E = R_angle +(h_0 * PHt_0);
//K=PH'/E
float K_0 = PHt_0 / E;
float K_1 = PHt_1 / E;
//P=P-KHP
//let Y=HP
float Y_0 = PHt_0;
float Y_1 = h_0 * P;
P -= K_0 * Y_0;
P -= K_0 * Y_1;
P -= K_1 * Y_0;
P -= K_1 * Y_1;
angle += K_0 * angle_err;
q_bias += K_1 * angle_err;
}
/**************************实现函数********************************************
*函数原型: void IMU_getYawPitchRoll(float * angles)
*功 能: 更新四元数 返回当前解算后的姿态数据
输入参数： 无
输出参数：没有
*******************************************************************************/
void IMU_getYawPitchRoll(float * angles)
{
IMU_getValues(mygetzhi);
stateUpdate(gyrox);
incAngleUpdate(mygetzhi);
kalmanUpdate(incAngle);
angles=incAngle;//航向
angles=angle;
angles =incAngle;//横滚
IMU_Yaw =angles;//航向
IMU_Pitch=angles;//俯仰
IMU_Roll =angles;//横滚
}
jj632856828 发表于 2013-11-6 10:44
自带dmp的
官方motion driver看不懂啊。。。一堆文件啊。。。 chushibinsaobao 发表于 2014-1-17 22:23
最近我也在学习卡尔曼滤波！从网上移植了个卡尔曼滤波，结果可以很好的跟踪角度，目前只能实现单个角度融合 ...
这位大哥，我采用IMUupdate这个算法（融合陀螺仪和加计的），可以得出很符合要求的Pitch和Roll，
但是没有想到好的办法得到在动态时符合要求的航向角，请问你有什么好的方法吗？
恳请赐教！！ MPU6050加速度读出原始三轴数据，怎样消除零偏啊，最后数字滤波是如何实现啊？有程序代码参考吗？ “MPU6050加速度读出原始三轴数据，怎样消除零偏啊，最后数字滤波是如何实现啊？有程序代码参考吗？”同问啊。
页:
[1]