卡尔曼滤波
这里的Pdot是一个中间变量，你只看几个是不可能会懂的，最好要全部一起看，下面是纤细步骤：
这里用到的kalman主要分为5个步骤：
(1) X=A*X+B*angular_speed_m;
(2) P=A*P*A'+Q;
(3) X=X-KG(Z-H*X);
(4) K=P*H'(H*P*H'+R);
(5) (I-KG*H)*P;
对矩阵P的更新只有 P=A*P*A'+Q; P,A,Q都是矩阵
A=1 -TS
0 1
Q=q_acce 0
0 q_gyro
单片机目前肯定是不能计算矩阵啦，说以就自己算矩阵乘法，加法(线性代数)
P[0][0]=(KAL_A[0][0]*P[0][0]+KAL_A[0][1]*P[1][0])*KAL_A[0][0]+(KAL_A[0][0]*P[0][1]+KAL_A[0][1]*P[1][1])*KAL_A[0][1]+KAL_Q[0][0];/////////////////////
P[0][1]=(KAL_A[0][0]*P[0][0]+KAL_A[0][1]*P[1][0])*KAL_A[1][0]+(KAL_A[0][0]*P[0][1]+KAL_A[0][1]*P[1][1])*KAL_A[1][1]+KAL_Q[0][1];////////////////////
P[1][0]=(KAL_A[1][0]*P[0][0]+KAL_A[1][1]*P[1][0])*KAL_A[0][0]+(KAL_A[1][0]*P[0][1]+KAL_A[1][1]*P[1][1])*KAL_A[0][1]+KAL_Q[1][0];////////////////////
P[1][1]=(KAL_A[1][0]*P[0][0]+KAL_A[1][1]*P[1][0])*KAL_A[1][0]+(KAL_A[1][0]*P[0][1]+KAL_A[1][1]*P[1][1])*KAL_A[1][1]+KAL_Q[1][1];////////////////////
然后把等于1或0的式子直接去掉
P[0][0]=(P[0][0]+KAL_A[0][1]*P[1][0])+(P[0][1]+KAL_A[0][1]*P[1][1])*KAL_A[0][1]+q_acce;// a
P[0][1]=P[0][1]+KAL_A[0][1]*P[1][1];// b
P[1][0]=P[1][0]+P[1][1]*KAL_A[0][1];// c
P[1][1]=P[1][1]+q_gyro;// d
下面是你写的
P[0][0] += Pdot[0] * dt;
P[1][1] += Pdot[3] * dt;
Pdot[0] = Q_angle - P[0][1] - P[1][0];
Pdot[3] = Q_gyro;
简化，约分后
P[0][0] +=（Q_angle - P[0][1] - P[1][0]）* dt； >>对应上面的a
P[1][1] +=Q_gyro * dt; >>对应上面的d
当然我自建的kalman和你看的那个版本还是有点差异，不过我的这个版本是可以用的，你看的哪个版本也可以用，效果都差不多。
卡尔曼滤波的具体原理
卡尔曼滤波是被斯坦利·施密特正式发现的，当时他在NASA埃姆斯研究中心的时候，发现自己的方法对于解决阿波罗计划的轨道预测是比较有用的，后来还根据研究最终发表了相关论文。
所谓的数据滤波是一种比较特别的，可以成功去除噪声还原真实数据的办法，这种特别的滤波在测量方差已知的时候可以更好的估计出动态系统。
卡尔曼滤波的完整体现
要知道传统的滤波方法，只能在信号和噪声有着不同频带的时候才可以真正实现。而卡尔曼滤波是一种比较新型的预测方法，比之前那种更加有效一些。
这种理论是在时间域上进行表述的，主要是在线性系统的状态空间的基础之上，最终可以求出系统状态的最优估计。最终就可以成功了解完整的行为。
卡尔曼滤波不会要求信号和噪声都是平稳的，只要有一定的假设，并且最终将含有噪音的信号进行一定处理，就可以得到误差最小的估算结果，和以前相比要简单方便很多，所以也获得了很大认可。
自从卡尔曼滤波正式推出，在很多领域都有所运用，比如通信系统、电力系统、航空航天等等，最终也获得了很多成果。
