卡尔曼滤波程序

代码说明：

卡尔曼滤波：以陀螺仪测量的角速度作为预测值的控制量，加速度传感器测量的角度作为观测值。 卡尔曼滤波是一个递推过程，初始的最优角度值可设为x=0，初始最优角度值的协方差p一定不能设为零，dt是采样周期，Q 与R可共同决定卡尔曼增益收敛的大小。