四元数更新算法 求助大神们看下结果有什么问题

dongdong602

我采用的四元数算法计算欧拉角，板子上的数据通过串口保存下来在在matlab上计算的，结果如附件，横轴是时间，纵轴是姿态角，三个依次是pit rol和yaw，

我是测试的时候将器件晃动了四次，每次都是缓慢向一个方向偏转然后再回复初始姿态。。

感觉计算出来的姿态角不对呀。。在晃动的时候是个震荡的曲线。。比如第一次算是正角度，更新后的第二次就成负的了

能帮我分析分析这是怎么回事么？这结果是正常的么？

dongdong602

补充一下传感器的输出数据，acc和gyr的数据

正点原子

帮顶

电子之旅

kp调大点试试

chb0613

用4阶龙格库塔法试试

dongdong602

电子之旅 发表于 2017-7-13 08:47
kp调大点试试

kp和ki目前看来影响有限。。。昨天我试了一下将误差积分先做一个低通滤波，再乘以kp加上去，出来的曲线没有震荡了，但是还是解不出正确的pit和rol角度

dongdong602

chb0613 发表于 2017-7-13 10:49
用4阶龙格库塔法试试

看到你的回复我决定去找下四阶的算法试试。。。。目前我很奇怪的点是为什么大家拿这个算法都很容易的能够解出pit和rol，然后去做pid。。而我其实就是照搬这套算法，而我的输入数据看起来也很正常，为什么我就解不出来呢？
希望大神也关注下这个帖子啊，我会及时更新最新的进展。。期待你的指导和交流

redawnlc

楼主问题解决了吗？
我觉得可以先不用高阶的R-K公式，也不用着急调Kp Ki
可以将PID参数设置为零，直接使用陀螺数据进行解算。如果你公式用的没错，而且陀螺零点补偿过的话。使用加计和陀螺解算的姿态角应该都不会有问题的，航向角应该有较小的漂移。
建议你保存下惯性器件原始数据，在MATLAB中算一下验证一下你的算法。
另外你的采样率和采样周期是多少？