MPU6050卡尔曼滤波读取数据问题

  是这样的，我在网上找到别人用卡尔曼滤波的方法处理6050的原始数据得出角度。试用之后发现当传感器静止不动时，读取的数据还算平稳也还准确，但是当我晃动传感器时再静止到一个位置，数据就是先振荡再平稳，振荡的时间还挺长的。比如我的传感器现在X轴角度为0°，我手动将传感器转到X轴角度为40°的位置，那么数据用上位机显示出来的就是  。先从0上升到一个大于40的角度，然后振荡最后平稳到40°。
如果说这个振荡持续的时间很短的话还没问题，可是我用上位机观察的话好持续好几秒，这样的话岂不是根本无法调试啊。