mup6050中的dmp与卡尔曼动态滤波的优劣

最近在网上买了一块6050模块，用店家的给的代码，他用的是6050自带的dmp来直接得到姿态的
可是，如果拿着模块在空中剧烈翻转的话，就会有二十多度的误差，如果用卡尔曼滤波的话，会不会有这么严重的误差呢？

DMP使用方便，直接调用函数接口，得出来的数据比较稳定，但是有误差。
卡尔曼滤波算法比较灵活，可以有比较好的解算结果，不过需要自己调参数。

用自带的DMP解算虽然缓解了主机的负担，但是数据更新的速率慢，而且误差大。一般最常用的就是把原始数据读出来在主机上进行姿态解算，这样就可以对不同的需求对算法进行改进，大幅度的提升精度。

楼主，有没有卡尔曼滤波的算法？求分享

同求卡尔曼滤波算法，谢谢大家

x_last = Data[0] ;
// x_now = x_last;
/* Output_Data = ((float)Value_sum/Cnt) * 5.01/65536; //采样求均值
我也在用这个，效果不是很好，我是直接把数据进行滤波后输出的
Value_sum =0; //AD_Null=0x00,AD_M=0x00,AD_H=0x00,AD_L=0x00,AD_E=0x00,AD_Value=0x00;
/
for(i=0;i<Cnt;i++){
x_now = x_last; //x_last=x(k-1|k-1),x_now=x(k|k-1)
p_now = p_last+ProcessNiose_Q; //p_now=p(k|k-1),p_last=p(k-1|k-1）
kg = p_now/(p_now+MeasureNoise_R); //kg为kalman filter，R为噪声
// z_measure=z_real+frand()*0.03;//测量值
x_next = x_now+kg(Data[i]-x_now);//估计出的最优值 x_now=x_now+kg*(*(Data+i)-x_now);
p_next = (1-kg)*p_now;//最优值对应的covariance
p_last = p_next; //更新covariance值
x_last = x_next; //更新系统状态值

在互补滤波中，通常使用加速度计和陀螺仪的数据来估计设备的方向，包括滚转（roll）、俯仰（pitch）和偏航（yaw）角度。滚转角是围绕X轴的旋转角度，俯仰角是围绕Y轴的旋转角度，而偏航角是围绕Z轴的旋转角度。

当设备只绕X轴旋转时，滚转角会发生变化，而俯仰角和偏航角通常不会发生显著变化。滚转角度是描述绕X轴旋转的角度，因此它会随着X轴的旋转而改变。但俯仰角和偏航角是与Y轴和Z轴的旋转相关的，所以它们在只绕X轴旋转时通常不会发生明显的变化。

互补滤波通过结合加速度计和陀螺仪的数据，可以估计设备的滚转、俯仰和偏航角度，但需要注意的是，在实际应用中，传感器数据可能会受到噪声和误差的影响，因此即使只有绕X轴的旋转，滚转角度也可能会略微变化。但这种变化通常会比较小，尤其是在设备保持相对稳定的情况下。

一阶互补滤波：
一阶互补滤波器的思想相对简单。它将陀螺仪的角速度数据与加速度计的角度数据结合起来，以估计物体的倾斜角度。具体来说，它会根据加速度计的读数来估计物体的绝对倾斜角度（例如，与水平面的夹角），然后使用陀螺仪的角速度数据来修正这个角度。这个修正是通过将陀螺仪的角速度积分（累积）来实现的。一阶互补滤波器对于简单的应用可能足够，但在某些情况下可能会导致滞后和不够平滑。

二阶互补滤波：
二阶互补滤波器相对复杂一些，但在某些情况下能够提供更好的性能。它不仅考虑角度的修正，还考虑了角速度的修正率。这可以帮助减小滞后并提供更平滑的姿态估计。

通俗地说，二阶互补滤波器更聪明，因为它考虑到了物体的角度变化速度。它使用了更多的信息，以更精确地估计物体的倾斜角度。这通常可以导致更平滑的姿态估计，特别是当物体正在加速或减速时。