一种采用加速收敛算法的定位方法及装置

摘要

本发明提出一种采用加速收敛算法的定位方法，包括以下步骤：获取加速度和地磁力的初始三维数值，计算出姿态四元数的初始值Q_A；获取角速度的三维数值，计算出姿态四元数的预测值Q_B=Q_A+dQ/dt；获取加速度和地磁力的当前三维数值，计算出姿态参数，所述姿态参数为罗德里格参数、方向余弦和欧拉角中的至少一种；通过相关的计算方法将计算得出的姿态参数转换为姿态四元数的观测值Q_D；对Q_D和Q_B进行融合计算，得到姿态四元数的融合值Q_E；再次获取角速度的三维数值，计算四元数的下一预测值Q_B1=Q_E+dQ/dt；重复以上相关步骤，获取四元数预测值Q_Bn。本发明采用了简单的运算方法得到定位结果，因此解决了由于运算问题产生的明显迟滞现象，能够实现快速收敛。