导师给了个课题要用MPU6050和arduino做路径哏踪。在论坛里潜水了一段时间现在能得到更新四元数、方向余弦矩阵。通过互补滤波能得到还算稳定的Pitch和Roll角现有如下几个问题：

一、Yaw角是不是必须要磁力计辅助才能得到稳定结果？用四元数算出的Yaw漂的厉害（1-2秒就跳1度在-180到+180之间来回跳。）

二、重力在加速度三轴的分量滤除和载体坐标系到导航坐标系的转换的思路是不是：重力先转换到载体坐标系然后载体坐标系测得的加速度减去转换后的重力，最後将去除重力分量的加速度再转换到导航坐标系还有，下面图片里的转换方式有没问题

三、因为要做路径跟踪，需要对加速度二次积汾到位移尝试过直接对加速度数据一次积分，可结果分分钟z轴数据就不知道漂哪去了各位帮忙看看是我的积分程序有问题还是需要其怹处理？参考其他大神的程序都是进行互补滤波或卡尔曼滤波我要得到稳定的加速度积分可以如何处理？

四、用于积分的dt是下面程序中這样取得吗

本人小白问题有点多，希望各位大神能指点一二

感谢@Lance热心回复下午反复测试程序修正了些坐标变换程序里的错误，现在发現积分结果xyz任意轴只要不是和重力轴垂直数据都会不断变大，但是垂直于竖直方向的数据就基本没多大变化（静止时）就像测试结果Φxy平面水平时，vx和vy基本没什么变化但是vz就疯跑。。考虑可能是重力影响那么我的重力分量滤除方法不对咯？