[发明专利]引入抗差估计的因子图融合定位方法

权利要求书

1.一种引入抗差估计的因子图融合定位方法，其特征在于，包括：

获取传感器量测信息；

基于所述量测信息确定出由位置、速度、姿态以及偏差量构成的多维状态量，构建状态空间模型，从而建立组合导航系统模型；

根据所述导航系统模型的预测残差向量构造检验统计量，判断是否有异常观测的存在；若存在，则引入指数衰减自适应因子自动调整观测噪声；若不存在，则构建因子图模型；

定义所述量测信息为因子图的因子节点，定义所述导航系统模型的状态量信息为因子图的变量节点，构建基于因子图的多源信息融合的系统框架图，从而构建因子图模型；

推算所述因子图模型，根据所述推算结果得到定位信息。

2.根据权利要求1所述的引入抗差估计的因子图融合定位方法，其特征在于，所述传感器量测信息，包括惯性测量单元中的陀螺仪和加速度计输出相应的角速度和比力的量测信息以及定位单元输出相应的经纬度量测信息。

3.根据权利要求1所述的引入抗差估计的因子图融合定位方法，其特征在于，所述状态空间模型为

式中，为k-1时刻的状态转移矩阵，H_k为k时刻的观测矩阵；W_k-1和V_k分别为状态噪声和观测噪声，Q_k-1,R_k为相应的协方差矩阵。

4.根据权利要求1所述的引入抗差估计的因子图融合定位方法，其特征在于，所述根据所述导航系统模型的预测残差向量构造检验统计量，判断是否有异常观测的存在为：

使用预测残差向量构造检验统计量，判断是否存在观测异常误差，从而判断是否有异常观测的存在。

5.根据权利要求4所述的引入抗差估计的因子图融合定位方法，其特征在于，所述使用预测残差向量构造检验统计量，判断是否存在观测异常误差，从而判断是否有异常观测的存在，包括：

设置置信度；

判断统计量是否超过置信度的极限，如超过置信度的极限，则存在观测异常误差。

6.根据权利要求4所述的引入抗差估计的因子图融合定位方法，其特征在于，所述置信度设置为：其中为显著性水平。

7.根据权利要求1所述的引入抗差估计的因子图融合定位方法，其特征在于，所述引入指数衰减自适应因子自动调整观测噪声，包括：

根据预测残差自动调整观测噪声，并设置噪声方差的上下限，当两个相邻的迭代差异未超过限制，则停止迭代。

8.根据权利要求1所述的引入抗差估计的因子图融合定位方法，其特征在于，定义所述量测信息为因子图的因子节点，定义所述导航系统模型的状态量信息为因子图的变量节点，构建基于因子图的多源信息融合的系统框架图，从而构建因子图模型为：

变量节点X，表示全局多元函数的变量，因子节点F，表示分解因子，变量节点X通过边E＝(e_ij)连接到因子节点F，边E＝(e_ij)是由变量节点X和因子节点F连线组成。

9.根据权利要求1所述的引入抗差估计的因子图融合定位方法，其特征在于，所述推算所述因子图模型，包括：

设置代价函数，将因子图模型消元变换为贝叶斯网络，通过高斯牛顿迭代法求解出状态更新量，最后得出最优估计值。

10.根据权利要求9所述的引入抗差估计的因子图融合定位方法，其特征在于，所述根据所述推算结果得到定位信息，包括：

基于最优估计值将获取的传感器量测信息进行融合，得到融合数据；

根据融合数据得到定位信息。