滤波的问题

1205889441@qq.c

我用卡尔曼滤波，试过了各种方法，现在的情况是这样的：不开电机用手摇车时感觉波形还是不错的，但是开了电机并且用手摆动车子的时候发现车子不跟手，加速度计波形比陀螺仪的积分滞后。下面图片中：红色为陀螺仪的积分值；黄色为滤波后的输出；粉色为加速度计的值。加速度计和陀螺仪的值我都做过处理，使它们的单位变为°和°/s。偶然中我发现了一种能让车子变硬且很跟手的办法，加大Ts,也就是陀螺仪采样间隔，但是这时的滤波输出波形非常差劲，是我以前绝对会摈弃的那种波形。可能我对于滤波的理解有问题，我说下我的想法，希望大家能帮我一下，我对于滤波的理解是这样的：陀螺仪的精度高但是会漂移，加速度计不会漂移但是噪声大，所以希望结合这两者的优点，使得输出波形在一定时间内在形状和绝对幅值上与陀螺仪的积分一致，但具体的数值与加速度计一致；在一段时间过后，滤波输出的角度值依然紧跟加速度计，输出角度的波形与陀螺仪积分的波形一致，但不会发生漂移。也就是说加速度计的值经过低通滤波器后其实跟陀螺仪的积分波形几乎一致，但陀螺仪会漂，这时的加速度计被滤除高频分量后相位会滞后。卡尔曼滤波算法中真正需要调节的是协方差矩阵Q和过程方差R,加速度计的噪声大，而陀螺仪的噪声小。（但是实际情况下，将车子静止在地面采集数据后，计算两者的方差，var(z_inc)=0.0049,var(u_gyro)=0.1114;在车子直立状态下，用手摇动车子采集数据并计算差，var(z_inc)= 10.8169,var(u_gyro)=103.9090;都不符合预期。采集的数值有问题？）最终我选取的两者方差之间差了3个数量级，过程的方差尝试后决定用1。卡尔曼滤波的K1，K2最后都会收敛，K2收敛为0，K1表现为对加速度计的信任程度，几次调试后选取K1=0.01，此时如果不开电机，手摇车子已经能够做到较好的效果，但在实际开电跑动的过程中效果并不理想。

1205889441@qq.c

解决了，谢谢各位。找到问题了，并不是我滤波的问题， 直立的P小了一个数量级。。。。

1205889441@qq.c

希望大家能够帮忙看看