一种基于卡尔曼滤波算法的三维定位方法及其定位系统

摘要

本发明一种基于卡尔曼滤波算法的三维定位方法及其定位系统，属于三维定位方法及其定位系统技术领域；所要解决的技术问题为：提供一种基于卡尔曼滤波算法的三维定位方法的改进；解决上述技术问题采用的技术方案为：获取定位目标所佩戴三维定位系统中的MEMS传感器采集的数据信息；对获取的传感器数据进行各自的误差补偿；采用四元数的扩展卡尔曼滤波定位算法对处理后的传感器数据误差进行修正；采用15元素的误差状态向量导航算法对传感器数据进行误差补偿；将步骤三、步骤四中经过修正和误差补偿的传感器数据输入卡尔曼滤波器进行滤波，输出定位目标的实时位置、姿态和速度信息；本发明应用于密闭、GPS信号较弱及网络信号较弱环境中的目标定位。