的方差值）Pkk1APk1k1A’Q这里的Q为向量
A
glecovA
gleA
glecovQ_biasA
gleQ_bias的协方差矩阵，即covA
gleQ_biascovQ_bias
covA
gleQ_bias0；
因为漂移噪声还有角度噪声是相互独立的，则
covQ_biasA
gle0
f又由性质可知cov（x，x）D（x）即方差，所以得到的矩阵如下
DA
gle00DQ_bias，这里的两个方差值是开始就给出的常数
程序中的定义如下floatQ_a
gle0001floatQ_gyro0003接着是这一部分APk1k1A’其中的（P（k1）Pk1）为上一时刻的预测方差阵卡尔曼滤波的目标就是要让这个预测方差阵最小。其中Pk1k1设为
ab1dt第一式已知A为cd01
则计算APk1k1A’Q（就是个矩阵乘法和加法，算算吧）结果如下
acdtbdtddt2DA
glebddtcddt
ddt2很小为了计算简便忽略不计。
于是得到
d
acdtbdtDA
glebddtcddtd
abcd分别和矩阵的P00P01P10P11计算过程转化为如下程序，代换即可Pdot0Q_a
glePP01PP10Pdot1PP11Pdot2PP11Pdot3Q_gyroPP00Pdot0dtPP01Pdot1dtPP10Pdot2dtPP11Pdot3dt三，这里是卡尔曼滤波的第三个式子KgkPkk1H’HPkk1H’R………3计算卡尔曼增益
f即计算卡尔曼增益，这是个二维向量设为
k0k1
，这里的
H

10为由此kg
PKK1R，这里又有一个常数R，程序中的定义如下floatR_a
gle05这个指的是角度测量噪声值，则式子的分母P00R_a
gle即程序中的PCt_0C_0PP00PCt_1C_0PP10ER_a
gleC_0PCt_0分子
P00P10
K0K于是求出1
K_0PCt_0EK_1PCt_1E四，用误差还有卡尔曼增益来修正XkkXkk1KgkZkHXkk1………4通过卡尔曼增益进行修正这个矩阵带进去就行了Z（kAccel注意这个是加速度计算出来的角度A
gle_errAccelA
gle对应程序如下A
gleQ_biasK_0A
gle_errK_1A
gle_err
同时为了PID控制还有下次的使用把角速度算出来了Gyro_xGyroQ_bias
五，最后一步对矩阵P进行更新，因为下一次滤波时要用到PP00K_0t_0PP01K_0t_1PP10K_1t_0PP11K_1t_1Pkk（IKgkH）Pkk1………5跟预测方差阵这个很简单，矩阵带进去算就行了
f六，总结卡尔曼滤波一共只需要给很少的初始值量，floatQ_a
gle0001floatQ_gyro0003还有floatR_a
gle05以及系统的初始量a
gle还有Q_bias还有预测误差矩阵P，程序里给的是0（数组）理论上由于卡尔曼滤波是迭代的算法，当时间充分长以后。滤波估值将与初始值r