卡尔曼滤波，输入的角度与角速度值到底应不应该减去零偏差？？

斩恨

       我的滤波效果一直不是很好，对卡尔曼滤波的写法不是很理解。
       从网上找了些卡尔曼程序，大体上都是先确定Q_angle，Q_gyro，R_angle的数值，然后输入加速度计测的角度，与陀螺仪测得的角速度，便得到滤波后的输出角度。
       但是滤出的效果一直不是很好。之前仿真发现陀螺仪静止的角速度值并不为零，我在想是不是卡尔曼输入的角速度是减去零偏差之后的角速度？还是要把Q_angle，Q_gyro，R_angle的数值改变？但又不知道向什么方向怎么改。
       还有，出现的波形含有严重的反冲现象，波形响应速度慢，有什么方法可以得到角度随时间实时变化的波形呢？？
       各位前辈，求教啊！！！