卡尔曼滤波 - mpu6050姿态融合原理及程序代码

　　三、卡尔曼滤波

　　其实卡尔曼滤波和一阶互补有些相似，输入也是一样的。卡尔曼原理以及什么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三个倾角，计算量比较大。一阶互补和卡尔曼滤波每次只能输出一个轴的姿态角。