传感器数据计算质心位移轨迹，结果不理想

背景如下：
想要使用惯性传感器数据（包含时间戳、三轴加速度、三轴陀螺仪数据、三轴磁力计数据、四元数）计算质心的位移轨迹。
计算步骤如下：

1、使用低通滤波对加速度、陀螺仪数据、磁力计数据 进行滤波处理
2、使用滤波后的数据，将四元数转换成旋转矩阵
3、利用旋转矩阵将加速度数据从传感器坐标系转换到全局坐标系，之后直接进行z轴的重力补偿
4、去除z轴的全局平均值
5、对加速度直接梯形积分得到速度
6、减去初始段的速度平均值，消除速度偏置
7、对速度梯形积分得到位移
8、 使用ZUPT优化位移结果
9、 绘制原始位移轨迹和优化后的位移轨迹

问题如下：
测试结果不理想，存在几厘米的差异，请问改进的方向都有什么呢？结果如下：