一、四元數法

　　關于四元數的一些概念和計算就不寫上來了，我也不懂。我能告訴你的是：通過下面的算法，可以把六個數據轉化成四元數（q0、q1、q2、q3），然后四元數轉化成歐拉角（P、R、Y角）。

　　雖然MPU6050自帶的DMP庫可以直接輸出四元數，減輕STM32的運算負擔，這里在此沒有使用，因為我是用STM32的硬件I2C讀取MPU6050數據的，DMP庫需要對I2C函數進行修改，如DMP庫中的I2C寫：i2c_write（st.hw-》addr，st.reg-》pwr_mgmt_1，1，&（data［0］））；有4個輸入變量，而STM32硬件I2C的I2C寫為：voidMPU6050_I2C_ByteWrite（u8slaveAddr，u8pBuffer，u8writeAddr），只有3個輸入量（這之間的差異好像是由于MPU6050的DMP庫是針對MSP430單片機寫的），所以必須進行修改，但是改固件庫是一件很痛苦的事，你們應該都懂。當然，如果你用模擬I2C的話，是容易實現的，網上的DMP移植幾乎都是基于模擬I2C的。

　　要注意的的是，四元數算法輸出的是三個量Pitch、Roll和Yaw，運算量很大。而像平衡小車這樣的例子只需要一個角（Pitch或Roll）就可以滿足工作要求，個人覺得做平衡小車最好不用四元數法。

　　#include《math.h》

　　#include“stm32f10x.h”

　　//------------------------

　　//變量定義

　　#defineKp100.0f//比例增益支配率收斂到加速度計/磁強計

　　#defineKi0.002f//積分增益支配率的陀螺儀偏見的銜接

　　#definehalfT0.001f//采樣周期的一半

　　floatq0=1，q1=0，q2=0，q3=0;//四元數的元素，代表估計方向

　　floatexInt=0，eyInt=0，ezInt=0;//按比例縮小積分誤差

　　floatYaw，Pitch，Roll;//偏航角，俯仰角，翻滾角

　　voidIMUupdate（floatgx，floatgy，floatgz，floatax，floatay，floataz）

　　{

　　floatnorm;

　　floatvx，vy，vz;

　　floatex，ey，ez;

　　//測量正常化

　　norm=sqrt（ax*ax+ay*ay+az*az）;

　　ax=ax/norm;//單位化

　　ay=ay/norm;

　　az=az/norm;

　　//估計方向的重力

　　vx=2*（q1*q3-q0*q2）;

　　vy=2*（q0*q1+q2*q3）;

　　vz=q0*q0-q1*q1-q2*q2+q3*q3;

　　//錯誤的領域和方向傳感器測量參考方向之間的交叉乘積的總和

　　ex=（ay*vz-az*vy）;

　　ey=（az*vx-ax*vz）;

　　ez=（ax*vy-ay*vx）;

　　//積分誤差比例積分增益

　　exInt=exInt+ex*Ki;

　　eyInt=eyInt+ey*Ki;

　　ezInt=ezInt+ez*Ki;

　　//調整后的陀螺儀測量

　　gx=gx+Kp*ex+exInt;

　　gy=gy+Kp*ey+eyInt;

　　gz=gz+Kp*ez+ezInt;

　　//整合四元數率和正常化

　　q0=q0+（-q1*gx-q2*gy-q3*gz）*halfT;

　　q1=q1+（q0*gx+q2*gz-q3*gy）*halfT;

　　q2=q2+（q0*gy-q1*gz+q3*gx）*halfT;

　　q3=q3+（q0*gz+q1*gy-q2*gx）*halfT;

　　//正常化四元

　　norm=sqrt（q0*q0+q1*q1+q2*q2+q3*q3）;

　　q0=q0/norm;

　　q1=q1/norm;

　　q2=q2/norm;

　　q3=q3/norm;

　　Pitch=asin（-2*q1*q3+2*q0*q2）*57.3;//pitch，轉換為度數

　　Roll=atan2（2*q2*q3+2*q0*q1，-2*q1*q1-2*q2*q2+1）*57.3;//rollv

　　//Yaw=atan2（2*（q1*q2+q0*q3），q0*q0+q1*q1-q2*q2-q3*q3）*57.3;//此處沒有價值，注掉

　　}