mpu6050融合算法

1. 使用MPU6050的DMP是不是可以不用互补滤波算法或者卡尔曼算法

给你arino的卡尔曼滤波融合算法，非原创，我只是封装了算法。 另外你这么难的问题应该给点分才厚道啊！ H文件： /* * KalmanFilter.h * Non-original * Author: x2d * Copyright (c) 2012 China * */ #ifndef KalmanFilter_h #define KalmanFi...

2. 谁能给我讲解一下卡尔曼滤波，我最近在用mpu6050，把陀螺仪和加速度的值通过卡尔曼滤波融合。求C程序!

给你arino的卡尔曼滤波融合算法，非原创，我只是封装了算法。
另外你这么难的问题应该给点分才厚道啊！

H文件：
/*
* KalmanFilter.h
* Non-original
* Author: x2d
* Copyright (c) 2012 China
*
*/

#ifndef KalmanFilter_h
#define KalmanFilter_h

#include <WProgram.h>

class KalmanFilter
{
public:
KalmanFilter();

/*
卡尔曼融合计算
angle_m: 加速度计测量并通过atan2(ax,ay)方法计算得到的角度（弧度值）
gyro_m:陀螺仪测量的角速度值（弧度值）
dt:采样时间(s)
outAngle:卡尔曼融合计算出的角度（弧度值）
outAngleDot:卡尔曼融合计算出的角速度（弧度值）
*/
void getValue(double angle_m, double gyro_m, double dt, double &outAngle, double &outAngleDot);

private:
double C_0, Q_angle, Q_gyro, R_angle;
double q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
double angle, angle_dot;
double P[2][2];
double Pdot[4];
};

CPP文件：
/*
* KalmanFilter.cpp
* Non-original
* Author: x2d
* Copyright (c) 2012 China
*
*/

#include "KalmanFilter.h"

KalmanFilter::KalmanFilter()
{
C_0 = 1.0f;
Q_angle = 0.001f;
Q_gyro = 0.003f;
R_angle = 0.5f;
q_bias = angle_err = PCt_0 = PCt_1 = E = K_0 = K_1 = t_0 = t_1 = 0.0f;
angle = angle_dot = 0.0f;
P[0][0] = 1.0f;
P[0][1] = 0.0f;
P[1][0] = 0.0f;
P[1][1] = 1.0f;
Pdot[0] = 0.0f;
Pdot[1] = 0.0f;
Pdot[2] = 0.0f;
Pdot[3] = 0.0f;
}

void KalmanFilter::getValue(double angle_m, double gyro_m, double dt, double &outAngle, double &outAngleDot)
{
/*
Serial.print("angle_m = ");
Serial.print(angle_m);
Serial.print(";");
Serial.print("gyro_m = ");
Serial.print(gyro_m);
Serial.print(";");
*/

angle+=(gyro_m-q_bias) * dt;
angle_err = angle_m - angle;
Pdot[0] = Q_angle - P[0][1] - P[1][0];
Pdot[1] = -P[1][1];
Pdot[2] = -P[1][1];
Pdot[3] = Q_gyro;
P[0][0] += Pdot[0] * dt;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[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 * P[0][1];
P[0][0] -= K_0 * t_0;
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;
angle += K_0 * angle_err;
q_bias += K_1 * angle_err;
angle_dot = gyro_m-q_bias;

outAngle = angle;
outAngleDot = angle_dot;
/*
Serial.print("angle = ");
Serial.print(angle);
Serial.print(";");
Serial.print("angle_dot = ");
Serial.print(angle_dot);
Serial.print(";");
*/
}
#endif

3. MPU6050读出的原始加速度数据和陀螺仪数据怎样处理

加速度计数据低通滤波
陀螺仪数据减去零偏值

之后再把两个数据进行融合，算出当前姿态

4. 请教，关于sensor/mpu6050

1）Arino官方视频教程
http://www.arino.cn/thread-2155-1-1.html

2）加速度计和陀螺仪指南
http://www.arino.cn/thread-3275-1-1.html
译文。文章主要介绍加速度计和陀螺仪的数学模型和基本算法，以及如何融合这两者，侧重算法、思想的讨论。
原文：http://www.starlino.com/imu_guide.html

5. MPU6050 加速度计 陀螺仪 PC机上实现数据融合算法

我知道的也不多，说一下我的理解吧。拿正点原子的程序为例mou6050：原子的程序配合上位机能输出6个数据，加速度输出：ax，ay，az角速度输出：wx，wy，wz分别在上位机上显示，这个数据是原始数据，dmp结算后的四元数。而单片机TFT屏幕上显示的Pitch、Roll、Yaw角度是通过陀螺仪的四元数解算出来，这个数据有个问题即使陀螺仪放不平（有个倾斜角），mpu6050上电后是以此时的状态为0度角度，这样测出来的数据肯定是错误的。原因是：陀螺仪测量的是角速度变化率，它也不知道0度在哪，它是以刷新的那个时刻记为0度开始积分的。因此陀螺仪单独是没法用的需要校准，校准的传感器可以是地磁传感器或者加速度计。而陀螺仪地磁和加速度计又有自己的缺点，需要他们把彼此的数据做个融合。

6. MPU6050+HMC5883L的数据融合，磁力计怎么加入算出YAW的准确角度

/**************************实现函数********************************************
*函数原型: void IMU_AHRSupdate
*功能: 更新AHRS 更新四元数
输入参数： 当前的测量值。
输出参数：没有
*******************************************************************************/
#define Kp 2.0f // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.01f // integral gain governs rate of convergence of gyroscope biases

void IMU_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float norm;
float hx, hy, hz, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez,halfT;
float tempq0,tempq1,tempq2,tempq3;

// 先把这些用得到的值算好
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q0q3 = q0*q3;
float q1q1 = q1*q1;
float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;

now = micros(); //读取时间
if(now<lastUpdate){ //定时器溢出过了。
halfT = ((float)(now + (0xffff- lastUpdate)) / 2000000.0f);
}
else {
halfT = ((float)(now - lastUpdate) / 2000000.0f);
}
lastUpdate = now; //更新时间

norm = invSqrt(ax*ax + ay*ay + az*az);
ax = ax * norm;
ay = ay * norm;
az = az * norm;
//把加计的三维向量转成单位向量。

norm = invSqrt(mx*mx + my*my + mz*mz);
mx = mx * norm;
my = my * norm;
mz = mz * norm;

/*
这是把四元数换算成《方向余弦矩阵》中的第三列的三个元素。
根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。
所以这里的vx\y\z，其实就是当前的欧拉角（即四元数）的机体坐标参照系上，换算出来的重力单位向量。
*/
// compute reference direction of flux
hx = 2*mx*(0.5f - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5f - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5f - q1q1 - q2q2);
bx = sqrt((hx*hx) + (hy*hy));
bz = hz;

// estimated direction of gravity and flux (v and w)
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);

// error is sum of cross proct between reference direction of fields and direction measured by sensors
ex = (ay*vz - az*vy) + (my*wz - mz*wy);
ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
ez = (ax*vy - ay*vx) + (mx*wy - my*wx);

/*
axyz是机体坐标参照系上，加速度计测出来的重力向量，也就是实际测出来的重力向量。
axyz是测量得到的重力向量，vxyz是陀螺积分后的姿态来推算出的重力向量，它们都是机体坐标参照系上的重力向量。
那它们之间的误差向量，就是陀螺积分后的姿态和加计测出来的姿态之间的误差。
向量间的误差，可以用向量叉积（也叫向量外积、叉乘）来表示，exyz就是两个重力向量的叉积。
这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。（你可以自己拿东西想象一下）由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。
*/
if(ex != 0.0f && ey != 0.0f && ez != 0.0f){
exInt = exInt + ex * Ki * halfT;
eyInt = eyInt + ey * Ki * halfT;
ezInt = ezInt + ez * Ki * halfT;

// 用叉积误差来做PI修正陀螺零偏
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;

}

// 四元数微分方程
tempq0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
tempq1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
tempq2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
tempq3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;

// 四元数规范化
norm = invSqrt(tempq0*tempq0 + tempq1*tempq1 + tempq2*tempq2 + tempq3*tempq3);
q0 = tempq0 * norm;
q1 = tempq1 * norm;
q2 = tempq2 * norm;
q3 = tempq3 * norm;
}

/**************************实现函数********************************************
*函数原型: void IMU_getYawPitchRoll(float * angles)
*功能: 更新四元数 返回当前解算后的姿态数据
输入参数： 将要存放姿态角的数组首地址
输出参数：没有
*******************************************************************************/
void IMU_getYawPitchRoll(float * angles) {
float q[4]; //四元数
volatile float gx=0.0, gy=0.0, gz=0.0; //估计重力方向
IMU_getQ(q); //更新全局四元数

angles[0] = -atan2(2 * q[1] * q[2] + 2 * q[0] * q[3], -2 * q[2]*q[2] - 2 * q[3] * q[3] + 1)* 180/M_PI; // yaw
angles[1] = -asin(-2 * q[1] * q[3] + 2 * q[0] * q[2])* 180/M_PI; // pitch
angles[2] = atan2(2 * q[2] * q[3] + 2 * q[0] * q[1], -2 * q[1] * q[1] - 2 * q[2] * q[2] + 1)* 180/M_PI; // roll
//if(angles[0]<0)angles[0]+=360.0f; //将 -+180度 转成0-360度
}

7. 求无人机悬停姿态数据

由于珠峰通航自身也从事无人机生产和无人机培训项目，因此也了解很多相关的数据资料，就简单的给楼主说一说！

1.视频的跟踪:这部分主要是云台的稳定问题，保证视频图像的稳定性，给下一步识别提供一个清晰准确的目标;

2.目标识别问题：这一部分就要涉及各种目标识别算法，特征提取等方面的内容;

3.飞控系统：这一部分就是姿态的解算以及电机的相关控制问题，包括完成相关的悬停等。

其实单独来看各个部分，都已经有产品问世，但是将其组合到一起来实现无人机的目标跟踪还是十分有难度的。也就是像大疆这样的nb公司才会有那么快的更新速度。

以上内容主要是说关于飞控部分的姿态控制问题。涉及到飞控一定会有姿态这个问题，通过看一些论文，发现大家广泛采用MPU6050这个东西，但是只用这个东西还不行，应该加上高度计，GPS模块和电子罗盘才行，这样才能够较为精确的得到飞行器的高度和水平位置信息。软件方面，姿态解算肯定首选卡尔曼滤波，但是在高度和水平位置这一块，卡尔曼滤波就不一定是最合适的了。对于高度测量由于是根据高度计和GPS两个传感器的数值进行融合得到，而两者的更新时间差距较大，卡尔曼滤波的优势就不是很明显，而互补滤波算法的处理时间则短得多,处理后的数据精度与卡尔曼滤波算法的精度相差无几，采用胡波滤波的方法。对于水平位置测量，则用电子罗盘对GPS的信息进行校正，提出双传感器融合算法.这样的的无人机的位置和姿态信息相对准确，通过协同作用，为pid算法提供相关电机调节参数。这些大概就是这一段时间的想法。

8. 新人求教 MPU6050 加速度读出后怎么滤波

MPU6050开发你可以去用带的DMP库，用这个库去处理数据，得到的数据就是融合后的

9. STM32读取HMC5883L的偏航角数据

HMC5883L是一种IIC通信的电子罗盘芯片，输出与北的角度偏差，数值是0-360度，靠西方向增长，可以理解为逆时针为正。配合GPS模块简直就是四轴飞行器的铁杆标配。

HMC5883L也是一个IIC器件，具体的操作方式和前面介绍的MPU6050类似，这里就不在赘述了。主要区别在于5883作为IIC从机时，其地址为0x3C。

初始化HMC5883的方法和初始化MPU6050的方法差不太多，主要是模拟出IIC信号，对对应的寄存器操作，具体的可查看HMC5883的说明手册。

取平均值之后，利用磁偏角公式可以求出角度。

如果前面MPU6050弄明白了，那5883也很容易弄懂。值得注意的有如下两个问题：

1. 接线

HMC5883和MPU605需要连接在同一个IIC总线上，下图是我们的接线方式。

最下方是没有用到的BMP模块

2. 四元数融合算法

要将HMC5883用到自主导航小车的姿态估算中，我们采用的是四元数融合算法，需要上传MPU6050和HMC5883的原始数据。如何进行四元数融合解算，我准备在下一篇文章写出。

本文首发https://blog.csdn.net/weixin_38172545/article/details/103044853

10. mpu6050用四元数数据融合俯仰角和横滚角都会飘

就是内部的运动引擎，直接输出四元数，可以减轻外围微处理器的工作负担且避免了繁琐的滤波和数据融合，DMP驱动是官方写的一个库，是430的，用来使用内肌单冠竿攉放圭虱氦僵部的dmp。 如果您认可我的答案，请采纳。 您的采纳，是我答题的动力。
根据官方资料，MPU6050只是输出陀螺和加速度计的6轴数据，然后使用arino通过自己的算法得到倾角。dmp通过使用MPU6050芯片中内置的 数据解算功能直接输出四元数、欧拉角等数据给 arino处理(这个功能官方没有正式公布)。