[发明专利]用于传感器融合定位的协方差确定方法、定位方法和装置

权利要求书

1.一种用于传感器融合定位的协方差确定方法，其中，包括：

参数编排步骤，包括：获取定位过程中当前迭代得到的变化步长大于设定步长阈值的传感器参数为第一参数；

雅克比矩阵解算步骤，包括：根据当前迭代得到的残差因子和参数编排步骤获得的第一参数，更新当前的雅克比矩阵，所述残差因子是根据传感器的测量数据和传感器参数确定的；

海森矩阵求解步骤，包括：根据更新后的雅克比矩阵中拆分的稠密子矩阵更新当前的海森矩阵，返回所述参数编排步骤，直至满足迭代结束条件，输出最终的海森矩阵；

分解步骤，包括：将海森矩阵分解得到上三角矩阵；

协方差解算步骤，包括：根据所述上三角矩阵通过递归的方式确定表征所述第一参数的准确度和相关性的协方差矩阵。

2.如权利要求1所述的方法，所述根据当前迭代得到的残差因子和参数编排步骤获得的第一参数，更新当前的雅克比矩阵，具体包括：

针对当前迭代得到的每个残差因子，根据该残差因子包含的第一参数，确定其在雅克比矩阵中对应元素的更新值；

根据该残差因子的标识和其包含的第一参数的标识，确定该残差因子在雅克比矩阵中对应元素的位置；

将当前的雅克比矩阵中所述位置的元素的值替换为相应的更新值。

3.如权利要求1所述的方法，所述根据更新后的雅克比矩阵中拆分的稠密子矩阵更新当前的海森矩阵，具体包括：

针对更新后的雅克比矩阵中的每一行，从第一个非零元素开始，将连续的至少两个非零元素拆分为稠密子矩阵，根据所述稠密子矩阵确定更新子元素的值；

根据更新子元素对应的第一参数的标识，确定该更新子元素在海森矩阵中对应元素的位置；

将当前的海森矩阵中所述位置的元素的值替换为对应的更新子元素的值之和。

4.如权利要求1所述的方法，所述根据所述上三角矩阵通过递归的方式确定表征所述第一参数的准确度和相关性的协方差矩阵，具体包括：

根据所述上三角矩阵中行列号最大的对角线元素的值，确定协方差矩阵中对应位置的元素的值，所述协方差矩阵中元素的总行数和总列数分别与所述上三角矩阵中元素的总行数和总列数一致；

按照行列号由大到小且先对角线元素后非对角线元素的顺序，通过递归的方式依次根据所述上三角矩阵中的相应元素的值和协方差矩阵中的相应元素的值，确定协方差矩阵中的未确定取值的元素的值。

5.如权利要求4所述的方法，所述通过递归的方式依次根据所述上三角矩阵中的相应元素的值和协方差矩阵中的相应元素的值，确定协方差矩阵中的未确定取值的元素的值，具体包括：

协方差矩阵中的未确定取值的元素为对角线元素时，确定该元素的第一行号和第一列号，根据上三角矩阵中行号与所述第一行号一样的元素的值，和协方差矩阵中的行号大于所述第一行号且列号与所述第一列号一样的元素的值，确定该未确定取值的元素的值；

协方差矩阵中的未确定取值的元素为对非角线元素时，确定该元素的第二行号和第二列号，根据上三角矩阵中行号与所述第二行号一样的元素的值，协方差矩阵中的行号大于所述第二行号且列号与所述第二列号一样的元素的值，和协方差矩阵中的列号与所述第二列号一样且行号比所述第二行号大的元素的值，确定该未确定取值的元素的值。

6.如权利要求1～5任一项所述的方法，还包括：

按设定规则从需要优化的传感器参数中确定目标参数；相应的，

所述获取定位过程中当前迭代得到的变化步长大于设定步长阈值的传感器参数为第一参数，具体包括：

从所述目标参数中获取定位过程中当前迭代得到的变化步长大于设定步长阈值的目标参数为第一参数。

7.如权利要求1～5任一项所述的方法，执行所述协方差解算步骤后，还包括：

根据所述协方差矩阵中的对角线元素，确定对应的传感器参数的准确度；和/或，

根据所述协方差矩阵中的元素，确定两两传感器参数的相关性。