一种基于滑动窗口的融合定位方法

本发明公开了一种基于滑动窗口的融合定位方法，包括：步骤1，将收集的传感器数据分别存入缓存队列中；步骤2，构建滑窗状态队列，将当前状态存入滑窗状态缓存队列中；步骤3，当传感器数据缓存队列有新数据时，取出队首数据作为当前处理的数据；对滑窗内的状态和协方差进行更新；步骤4，根据相对位姿观测数据的缓存队列有新数据时，循环取出队首数据作为当前处理的数据，对滑窗内对应时刻的状态和协方差进行更新；步骤5，输出更新后的状态量和协方差；步骤6，将更新后的状态量和协方差输出，完成车辆状态的估计。本发明能够有效地提高智能驾驶车辆在复杂环境下的定位精度和稳定性，为智能驾驶的安全性和可靠性提供有力保障。