對於加速計與陀螺儀融合，介紹兩種濾波算法：他們的核心思想不太相同，這兩種方法現在也已經有了應用，現在你可以發現互補濾波融合在無人機上，EKF我做過在平衡車上面。mathony主要思想是用四元數來更新，最後轉化爲歐拉角。EKF主要是根據角度與角速度建立陀螺儀的模型，通過加速度算出角度來更新陀螺儀的累積誤差。
1.首先介紹一下互補濾波算法：
先定義Kp，Ki，以及halfT 。
Kp，Ki，控制加速度計修正陀螺儀積分姿態的速度
halfT ，姿態解算時間的一半。此處解算姿態速度爲500HZ，因此halfT 爲0.001
#define Kp 2.0f
#define Ki 0.002f
#define halfT 0.001f
初始化四元數
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
定義姿態解算誤差的積分
float exInt = 0, eyInt = 0, ezInt = 0;
以下爲姿態解算函數。
參數gx，gy，gz分別對應三個軸的角速度，單位是弧度/秒;
參數ax，ay，az分別對應三個軸的加速度原始數據
由於加速度的噪聲較大，此處應採用濾波後的數據
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
float vx, vy, vz;
float ex, ey, ez;
將加速度的原始數據，歸一化，得到單位加速度
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
把四元數換算成“方向餘弦矩陣”中的第三列的三個元素。根據餘弦矩陣和歐拉角的定義，地理座標系的重力向量，轉到機體座標系，正好是這三個元素。所以這裏的vx、vy、vz，其實就是當前的機體座標參照系上，換算出來的重力單位向量。(用表示機體姿態的四元數進行換算)
vx = 2*(q1*q3 – q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 – q1*q1 – q2*q2 + q3*q3;
這裏說明一點，加速度計由於噪聲比較大，而且在飛行過程中，受機體振動影響比陀螺儀明顯，短時間內的可靠性不高。陀螺儀噪聲小，但是由於積分是離散的，長時間的積分會出現漂移的情況，因此需要將用加速度計求得的姿態來矯正陀螺儀積分姿態的漂移。
在機體座標參照系上，加速度計測出來的重力向量是ax、ay、az;陀螺積分後的姿態來推算出的重力向量是vx、vy、vz;它們之間的誤差向量，就是陀螺積分後的姿態和加速度計測出來的姿態之間的誤差。
向量間的誤差，可以用向量積(也叫外積、叉乘)來表示，ex、ey、ez就是兩個重力向量的叉積。這個叉積向量仍舊是位於機體座標系上的，而陀螺積分誤差也是在機體座標系，而且叉積的大小與陀螺積分誤差成正比，正好拿來糾正陀螺。由於陀螺是對機體直接積分，所以對陀螺的糾正量會直接體現在對機體座標系的糾正。簡單的講就是說本來陀螺儀轉換的出的重力向量應該和加速度計重力向量相等的，但是現在可能由於加速度計積分誤差導致不相等，所以需要校正。
叉乘是數學基礎，百度百科裏有詳細解釋。
ex = (ay*vz – az*vy);
ey = (az*vx – ax*vz);
ez = (ax*vy – ay*vx);
將叉乘誤差進行積分
exInt = exInt + ex*Ki*halfT;
eyInt = eyInt + ey*Ki*halfT;
ezInt = ezInt + ez*Ki*halfT;
用叉乘誤差來做PI修正陀螺零偏，通過調節Kp，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;
最後根據四元數方向餘弦陣和歐拉角的轉換關係，把四元數轉換成歐拉角。所以有：
ANGLE.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw
ANGLE.Y= asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
ANGLE.X= atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
}
2.EKF濾波算法
1.這裏，因爲用在平衡車上，所以只需要一個傾角模型就可以。根據上一時刻角度值得到了當前角度的預測值 Angle(k+1)：
Angle(k)=Angle(k-1)+(Gyro - Q_bias) * dt （1）
其中等號左邊Angle(k)爲此時的角度，等號右邊Angle(k-1)爲上一時刻的角度，Gyro 爲陀螺儀測的角速度的值，dt是兩次濾波之間的時間間隔,float dt=0.005; 這是程序中的定同時 Q_bias也是一個變化的量,即使漂移誤差。
通過上面（1）式即可得到下面矩陣形式：
2.
3
.
4.
代碼如下：
void Kalman_Filter(float Gyro,float Accel)
{
Angle+=(Gyro - Q_bias) * dt;
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; /
Pdot[1]= - PP[1][1];
Pdot[2]= - PP[1][1];/
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt;
PP[0][1] += Pdot[1] * dt;
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - Angle;
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_err;
Q_bias += K_1 * Angle_err;
Gyro_x = Gyro - Q_bias;
}
參考文章地址：
1.http://bbs.eeworld.com.cn/home.php?do=blog&id=517775&mod=space&uid=779081