三、卡爾曼濾波

　　其實卡爾曼濾波和一階互補有些相似，輸入也是一樣的。卡爾曼原理以及什么5個公式等等的，我也不太懂，就不寫了，感興趣的話可以上網查。在此給出具體程序，和一階互補算法一樣，每次卡爾曼濾波只能得到一個方向的角度。

　　#include《math.h》

　　#include“stm32f10x.h”

　　#include“Kalman_Filter.h”

　　//卡爾曼濾波參數與函數

　　floatdt=0.001;//注意：dt的取值為kalman濾波器采樣時間

　　floatangle，angle_dot;//角度和角速度

　　floatP［2］［2］={{1，0}，

　　{0，1}};

　　floatPdot［4］={0，0，0，0};

　　floatQ_angle=0.001，Q_gyro=0.005;//角度數據置信度，角速度數據置信度

　　floatR_angle=0.5，C_0=1;

　　floatq_bias，angle_err，PCt_0，PCt_1，E，K_0，K_1，t_0，t_1;

　　//卡爾曼濾波

　　floatKalman_Filter（floatangle_m，floatgyro_m）//angleAx和gyroGy

　　{

　　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;//最優角速度

　　returnangle；

　　}

　總結：三種融合算法都能夠輸出姿態角（Pitch和Roll），一次四元數法可以輸出P、R、Y三個傾角，計算量比較大。一階互補和卡爾曼濾波每次只能輸出一個軸的姿態角。