为什么卡尔曼滤波器失灵了？

二十几天前写的卡尔曼滤波库，之前是好用的，现在调用后发现输出的角度抖动很厉害，不像滤波后的数据。

.h文件

#ifndef KALMAN_H
#define KANMAN_H

class Kalman{
  public:
    Kalman();                                                                       //构造函数 初始化对象

    float Kalman_Filter(float newAngle,float newGyro);    //获取并处理mpu6050信息
    
  private:
    float Q_angle; 				         //加速度计噪声协方差
    float Q_gyro;       		                         //陀螺仪噪声协方差
    float R_angle;      			         //角度测量噪声协方差

    float angle;       				         //最终得到的角度
    float bias;         				         //陀螺仪零偏
    float gyro;         				         //最终得到的角速度

    float dt;          				         //卡尔曼滤波采样时间

    float p[2][2];      			                         //先验或最优估计协方差矩阵
};
#endif

.cpp文件

#if ARDUINO >= 100
  #include "Arduino.h"
#else
  #include "WProgram.h"
#endif
#include "Kalman.h"

Kalman::Kalman()
{
  Q_angle = 0.001f;
  Q_gyro = 0.003f;
  R_angle = 0.03f;

  angle = 0.0f;
  bias = 0.0f;

  dt = 0.0055f;

  p[0][0] = 1.0f;
  p[0][1] = 0.0f;
  p[1][0] = 0.0f;
  p[1][1] = 1.0f;        //p初始值取1
}

float Kalman::Kalman_Filter(float newAngle,float newGyro)
{
  float x = p[0][0] + R_angle;
  float K[2];
  float y = newAngle - angle;
  gyro = newGyro - bias;
  angle += gyro * dt;                     //预测当前角度
  p[0][0] += (Q_angle - p[0][1] - p[1][0]) * dt;
  p[0][1] -= p[1][1] * dt;
  p[1][0] -= p[1][1] * dt;
  p[1][1] += Q_gyro * dt;                 //计算预测估计协方差矩阵

  K[0] = p[0][0] / x;      
  K[1] = p[1][0] / x;                     //计算卡尔曼增益

  angle += K[0] * y;
  bias += K[1] * y;                       //计算当前最优估计值

  float Plast0 = p[0][0];
  float Plast1 = p[0][1];

  p[0][0] -= K[0] * Plast0;
  p[0][1] -= K[0] * Plast1;
  p[1][0] -= K[1] * Plast0;
  p[1][1] -= K[1] * Plast1;               //更新协方差矩阵

  return angle;
}