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處理(這個功能官方沒有正式公布)。