基于因子图的车辆集群导航源数据融合协同定位方法

本发明提供了基于因子图的车辆集群导航源数据融合协同定位方法，首先构建车辆集群分布式协同定位系统模型，获取定位参数并计算得到车辆导航源定位结果及其三维协方差矩阵；然后获取车辆间测距测向信息，并结合得到的车辆导航源定位结果，获取车辆内部因子图，计算协同车辆对待定位车辆的预测定位结果及其三维协方差矩阵；最后对预测定位结果进行正交分解，获得各个正交轴上的定位数据，对各轴定位数据集合进行分级筛选，识别并剔除误差较大的导航源定位数据，利用筛选后各轴数据对因子图参数更新，得到车辆集群网络中车辆融合协同定位结果。本发明方法适用于复杂环境下的大规模车辆集群导航定位需求，并且得到的定位结果的稳定性和鲁棒性高。