用于传感器融合定位的协方差确定方法、定位方法和装置与流程
本公开涉及定位技术领域，特别涉及用于传感器融合定位的协方差确定方法、定位方法和装置。
背景技术：
多传感器融合定位通常是指包括视觉传感器(如相机)、惯性测量单元(inertialmeasurementunit，imu，简称“惯导”)和全球导航卫星系统(globalnavigationsatellitesystem，gnss)定位等的定位系统，该系统兼具定位精度高和成本低的优势，成为了需要高精度(厘米级)定位结果的应用场景主要选择的定位方案。通过传感器融合定位获得高精度定位结果的过程中，需要确定各传感器的位置和姿态等参数，以及确定这些参数的协方差，以便对融合定位的定位精度半径进行估计和对相关性进行评估等。
现有技术一般采用海森矩阵求逆的方法进行协方差估计，但多传感器融合定位的参数的维数较多，直接矩阵求逆的算力消耗巨大，导致系统成本升高，而为了降低成本，进行近似处理又会影响估计出的协方差的准确性。因此，需要提供新的协方差确定方案，以满足成本和准确性的要求。
技术实现要素：
鉴于上述问题，提出了本公开以便提供一种克服上述问题或者至少部分地解决上述问题的用于传感器融合定位的协方差确定方法、定位方法和装置。
第一方面，本公开实施例提供一种用于传感器融合定位的协方差确定方法，包括：
参数编排步骤，包括：获取定位过程中当前迭代得到的变化步长大于设定步长阈值的传感器参数为第一参数；
雅克比矩阵解算步骤，包括：根据当前迭代得到的残差因子和参数编排步骤获得的第一参数，更新当前的雅克比矩阵，所述残差因子是根据传感器的测量数据和传感器参数确定的；
海森矩阵求解步骤，包括：根据更新后的雅克比矩阵中拆分的稠密子矩阵更新当前的海森矩阵，返回所述参数编排步骤，直至满足迭代结束条件，输出最终的海森矩阵；
分解步骤，包括：将海森矩阵分解得到上三角矩阵；
协方差解算步骤，包括：根据所述上三角矩阵通过递归的方式确定表征所述第一参数的准确度和相关性的协方差矩阵。
第二方面，本公开实施例提供一种传感器融合定位方法，包括：
根据按照上述方法确定的协方差矩阵中的对角线元素，确定对应的传感器参数的准确度；
将准确度大于设定阈值的参数作为下一步传感器融合定位中对应参数的初值。
第三方面，本公开实施例提供一种用于传感器融合定位的协方差确定装置，包括：
参数编排模块，用于获取定位过程中当前迭代得到的变化步长大于设定步长阈值的传感器参数为第一参数；
雅克比矩阵解算模块，用于根据当前迭代得到的残差因子和所述参数编排模块获得的第一参数，更新当前的雅克比矩阵，所述残差因子是根据多传感器的测量数据和传感器参数确定的；
海森矩阵求解模块，用于根据所述雅克比矩阵解算模块得到的更新后的雅克比矩阵中拆分的稠密子矩阵更新当前的海森矩阵；所述参数编排模块、雅克比矩阵解算模块和海森矩阵求解模块循环工作，直至迭代结束，所述海森矩阵求解模块，还用于输出最终的海森矩阵；
分解模块，用于将所述海森矩阵求解模块输出的海森矩阵分解得到上三角矩阵；
协方差解算模块，用于根据所述分解模块得到的上三角矩阵通过递归的方式确定表征所述第一参数的准确度和相关性的协方差矩阵
第四方面，本公开实施例提供一种计算机程序产品，包括计算机程序/指令，其中，该计算机程序/指令被处理器执行时实现上述用于传感器融合定位的协方差确定方法，或实现上述传感器融合定位方法。
本公开实施例提供的用于传感器融合定位的协方差确定方法，包括：参数编排步骤，获取定位过程中当前迭代得到的变化步长大于设定步长阈值的传感器参数为第一参数；雅克比矩阵解算步骤，根据当前迭代得到的残差因子和参数编排步骤获得的第一参数，更新当前的雅克比矩阵，残差因子是根据多传感器的测量数据和传感器参数确定的；海森矩阵求解步骤，根据更新后的雅克比矩阵中拆分的稠密子矩阵更新当前的海森矩阵，返回参数编排步骤，直至迭代结束，得到最终的海森矩阵；分解步骤，将海森矩阵分解得到上三角矩阵；协方差解算步骤，根据上三角矩阵通过递归的方式确定表征第一参数的准确度和相关性的协方差矩阵。上述技术方案的有益效果至少包括：
(1)根据当前迭代得到的传感器参数的变化步长，筛选变化步长大于设定步长阈值的参数为第一参数，根据当前迭代得到的第一参数和残差因子，更新当前的雅克比矩阵。每次迭代完后都利用上次迭代完后雅克比矩阵的更新结果，只更新其中变化步长较大的传感器参数对应的元素，减少了计算量；根据雅克比矩阵中拆分的稠密子矩阵更新当前的海森矩阵，省去了稀疏块的计算量，较通过对残差因子求二阶导的方式，或者是直接根据雅克比矩阵和其转置的乘积来确定海森矩阵的方式，进一步减少了计算量；将海森矩阵分解得到上三角矩阵，再通过递归的方式依次确定协方差矩阵中的每个元素，较直接利用海森矩阵求逆的方式减少了计算量。
(2)本方案通过增量更新雅克比矩阵和海森矩阵，递归确定协方差矩阵中的每个元素的方式，只是转换了确定方法，即采用增量递归的方法来确定各传感器参数的协方差矩阵，确定过程中并未做近似处理，所以在减少计算量的同时，保证了准确度。
本公开的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本公开而了解。本公开的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。
下面通过附图和实施例，对本公开的技术方案做进一步的详细描述。
附图说明
附图用来提供对本公开的进一步理解，并且构成说明书的一部分，与本公开的实施例一起用于解释本公开，并不构成对本公开的限制。在附图中：
图1为本公开实施例一中用于传感器融合定位的协方差确定方法的流程图；
图2为图1中步骤s12的具体实现流程图；
图3为图1中步骤s13的具体实现流程图；
图4为本公开实施例二中用于传感器融合定位的协方差确定方法的具体实现流程图；
图5为本公开实施例中传感器参数块的编排示例图；
图6为本公开实施例中根据雅可比矩阵增量确定海森矩阵的示例图；
图7为本公开实施例中用于传感器融合定位的协方差确定装置的结构示意图。
具体实施方式
下面将参照附图更详细地描述本公开的示例性实施例。虽然附图中显示了本公开的示例性实施例，然而应当理解，可以以各种形式实现本公开而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本公开，并且能够将本公开的范围完整的传达给本领域的技术人员。
为了解决现有技术中存在的多传感器融合定位系统定位参数的协方差确定过程算力消耗大和精度低的问题，本公开实施例提供用于传感器融合定位的协方差确定方法、定位方法和装置，能够较为准确的进行协方差的恢复，且整个过程的计算量相对较小。
下述实施例中的多传感器指多传感器融合定位系统，通常包括视觉传感器、惯导和全球导航卫星系统，利用多传感器定位需要根据多传感器的测量数据通过迭代优化的方式确定传感器参数，即定位过程中所需要优化的与传感器相关的定位参数，也可简称为定位参数。具体的，传感器参数可以是直接的位姿参数，也可以是用于确定位姿参数过程中的其他待优化参数，例如，初始化时迭代优化的传感器参数包括：视觉传感器的位姿(每一帧相对于第一帧的位置和姿态)、视觉传感器的尺度、视觉传感器的速度、相机坐标系与世界坐标系间的旋转外参和陀螺仪零偏等；定位过程中迭代优化的传感器参数包括：相对世界坐标系下的位姿、绝对世界坐标系下的位姿、相对世界坐标系与绝对世界坐标系间的旋转外参和全球定位系统的天线与惯导间的相对位置外参等。迭代优化过程中需要建立包含各传感器的测量数据即观测数据和待优化的传感器参数的多个残差因子，每个残差因子可以是包含部分的观测数据和部分的传感器参数。
传感器参数x＝(x1,x2,……，xn)t为n维参数时，其协方差矩阵为：
其中，∑ij为第i个传感器参数与第j个传感器参数间的协方差，即为协方差矩阵中第i行、第j列的元素，i＝1,2……n，j＝1,2……n。
传感器参数的协方差矩阵的确定可以是先根据传感器参数和残差因子确定雅克比矩阵jacobiansmatrix，再根据雅克比矩阵确定海森矩阵hessianmatrix，最后根据海森矩阵确定协方差矩阵。
在向量微积分中，雅可比矩阵是一阶偏导数以一定方式排列成的矩阵，其行列式称为雅可比行列式。雅可比矩阵的重要性在于它体现了一个可微方程与给出点的较优线性逼近。
假设有m个残差因子r1,r2,……，rm，n个传感器参数x1,x2,……，xn，则确定的雅可比矩阵j为：
其中，jti表示雅克比矩阵中第t行第i列的元素，即第t个残差因子rt关于第i个传感器参数xi的一阶偏导数，t＝1,2……m，i＝1,2……n。
海森矩阵是一个多元函数的二阶偏导数构成的方阵，描述了函数的局部曲率。假设残差因子构成的目标因子(即各残差因子的总和)为r，n个传感器参数x1,x2,……，xn，则确定的海森矩阵h为：
其中，hij表示目标因子r关于第i个传感器参数和第j个传感器参数的二阶偏导数，i＝1,2……n，j＝1,2……n。
具体传感器参数的协方差确定方法，后续实施例中详细介绍。
实施例一
本公开实施例一提供一种用于传感器融合定位的协方差确定方法，其流程如图1所示，包括如下步骤：
步骤s11：参数编排。
具体包括：获取定位过程中当前迭代得到的变化步长大于设定步长阈值的传感器参数为第一参数。
定位过程中需要对各传感器参数进行迭代优化在传感器参数的每次迭代优化过程中，可能只有一小部分参数发生了变化，意味着很多的参数不需要重新进行线性化，所以针对迭代优化中步长变化小的传感器参数，其对应的残差因子也不需要进行重新线性化，故不需要更新其在雅克比矩阵中的对应元素的值，这可以大大降低算力消耗。
步骤s12：雅克比矩阵解算。
具体包括：根据当前迭代得到的残差因子和参数编排步骤获得的第一参数，更新当前的雅克比矩阵，其中，残差因子是根据传感器的测量数据和传感器参数确定的。
在一个实施例中，参照图2所示，雅克比矩阵的更新可以包括下述步骤：
步骤s121：针对当前迭代得到的每个残差因子，根据该残差因子包含的第一参数，确定其在雅克比矩阵中对应元素的更新值。
每个残差因子可能包含一个或多个第一参数，针对每个残差因子，根据该残差因子包含的各第一参数在当前迭代的迭代结果，分别确定该残差因子关于各第一参数的一阶导数，作为该残差因子和其包含的每个第一参数在雅克比矩阵中对应元素的更新值。具体的，一个残差因子和其包含的一个第一参数在雅克比矩阵中对应一个元素。
步骤s122：根据该残差因子的标识和其包含的第一参数的标识，确定该残差因子在雅克比矩阵中对应元素的位置。
步骤s123：将当前的雅克比矩阵中所述位置的元素的值替换为相应的更新值。
例如残差因子r2包含第一参数x1和x5，则可以确定r2关于x1的一阶导数作为残差因子r2和第一参数x1在雅克比矩阵中对应元素a的更新值，确定r2关于x5的一阶导数作为残差因子r2和第一参数x5在雅克比矩阵中对应元素b的更新值；根据该残差因子的标识(下标2)和其包含的第一参数x1的标识(下标1)，确定元素a在下述雅克比矩阵中的位置为第2行第1列，根据该残差因子的标识(下标2)和其包含的第一参数x5的标识(下标5)，确定元素b在下述雅克比矩阵中的位置为第2行第5列。将当前雅克比矩阵中的第2行第1列的元素的值替换为元素a的更新值，第2行第5列的元素值替换为元素b的更新值；因为残差因子r2不包含其他的残差因子，故雅克比矩阵中第2行其他的元素不做变化。
其中，jti表示雅克比矩阵中第t行第i列的元素，即第t个残差因子rt关于第i个传感器参数xi的一阶偏导数，t＝1,2……m，i＝1,2……n。
步骤s13：海森矩阵求解。
具体包括：根据更新后的雅克比矩阵中拆分的稠密子矩阵更新当前的海森矩阵。
可以将雅克比矩阵与雅克比矩阵的转置矩阵的乘积作为海森矩阵，但该方式的复杂度很高，例如m维残差因子，n维传感器参数时，整体复杂度为(mn)2。由于每个残差因子往往只包含很少的传感器参数，故雅克比矩阵中的很多元素都为0，即雅克比矩阵是稀疏的，故可以通过增量式的方式，进行快速的海森矩阵的更新。
具体的，上述稠密子矩阵指非0元素较多的矩阵。
在一个实施例中，参照图3所示，根据更新后的雅克比矩阵中拆分的稠密子矩阵更新当前的海森矩阵，可以包括下述步骤：
步骤s131：针对更新后的雅克比矩阵中的每一行，从第一个非零元素开始，将连续的至少两个非零元素拆分为稠密子矩阵，根据稠密子矩阵确定更新子元素的值。
具体的，针对雅克比矩阵中的每一行，可以是拆分出一个稠密子矩阵，也可以是拆分出多个稠密子矩阵，例如，从第一个非零元素开始，以零元素为间隔，将连续的至少两个非零元素拆分为一个稠密子矩阵，若一个非零元素的下一元素为零元素，则将该非零元素与后续连续的非零元素划分为一个稠密子矩阵。
根据稠密子矩阵确定更新子元素的值，可以是先确定稠密子矩阵与稠密子矩阵转置的乘积，确定其中的非零元素为更新子元素的值。
步骤s132：根据更新子元素对应的第一参数的标识，确定该更新子元素在海森矩阵中对应元素的位置。
具体的，每个更新子元素的值是根据雅克比矩阵中的两个元素的值得到的，可以是雅克比矩阵中两个相同的元素，也可以是两个不同的元素；雅克比矩阵中的元素对应有唯一的第一参数的标识，根据两个第一参数的标识确定更新子元素在海森矩阵中对应元素的位置。
例如，稠密子矩阵a为
步骤s133：将当前的海森矩阵中该位置的元素的值替换为对应的更新子元素的值之和。
将每个残差因子对应的雅可比矩阵中的元素拆解成多个稠密的增量块，在稀疏海森矩阵上进行多个增量式稠密子矩阵的更新；结合步骤s11中稳定的参数，即步长变化小的第一参数在雅可比矩阵中的对应元素不做更新，增量式更新雅可比矩阵和海森矩阵，协方差确定效率得以提升。
融合定位是通过迭代优化的方法确定传感器参数，在一次优化的每次迭代后，都需要更新当前的雅可比矩阵和海森矩阵，迭代结束后根据最终的海森矩阵恢复协方差矩阵。故，执行完步骤s13后，需确定是否满足迭代结束条件，若是，执行步骤s14；若否，返回步骤s11，直至满足迭代结束条件。
步骤s14：输出最终的海森矩阵。
步骤s15：海森矩阵分解。
具体包括：将海森矩阵分解得到上三角矩阵。
在一个实施例中，可以是，将海森矩阵进行cholesky分解得到上三角矩阵。将海森矩阵分解为一个上三角矩阵和一个下三角矩阵的乘积，得到上三角矩阵。
步骤s16：协方差解算。
具体包括，根据上三角矩阵通过递归的方式确定表征第一参数的准确度和相关性的协方差矩阵。
利用海森矩阵求逆的方式确定协方差矩阵，由于第一参数的维数较多，会造成很大的算力消耗，故可以通过递归的方式，逐步确定协方差矩阵中的元素的值。
在一个实施例中，可以是，按照行列号由大到小且先对角线元素后非对角线元素的顺序，通过递归的方式依次根据上三角矩阵中的相应元素的值和协方差矩阵中的已确定取值的相应元素的值，确定协方差矩阵中的未确定取值的元素的值。协方差矩阵中元素的总行数和总列数分别与上三角矩阵中元素的总行数和总列数一致。
具体的，首先根据上三角矩阵中行列号最大的对角线元素的值，确定协方差矩阵中对应位置的元素的值，即先根据上三角矩阵中行列号最大的对角线元素的值，第一次确定协方差矩阵中对应位置的为确定取值的元素的值；再按照行列号由大到小且先对角线元素后非对角线元素的顺序，通过递归的方式依次根据上三角矩阵中的相应元素的值和协方差矩阵中的相应元素的值，确定协方差矩阵中的未确定取值的元素的值。最终确定完协方差矩阵中每个元素的取值，完成协方差矩阵的更新。
具体的，上述协方差矩阵，其对角线元素表征第一参数的准确度，即第一元素的精度半径，故可以根据协方差矩阵中的对角线元素，确定对应的传感器参数的准确度；协方差矩阵中每一元素表征相应的两个传感器参数的相关性，故可以根据协方差矩阵中的元素，确定两两传感器参数的相关性。
上述实施例一中技术方案的有益效果至少包括：
(1)根据当前迭代得到的传感器参数的变化步长，筛选变化步长大于设定步长阈值的传感器参数为第一参数，根据当前迭代得到的第一参数和残差因子，更新当前的雅克比矩阵。每次迭代完后都利用上次迭代完后雅克比矩阵的更新结果，只更新其中变化步长较大的传感器参数对应的元素，减少了计算量。
(2)根据雅克比矩阵中拆分的稠密子矩阵更新当前的海森矩阵，省去了稀疏块的计算量，较通过对残差因子求二阶导的方式，或者是直接根据雅克比矩阵和其转置的乘积来确定海森矩阵的方式，进一步减少了计算量。
(3)将海森矩阵分解得到上三角矩阵，再通过递归的方式依次确定协方差矩阵中的每个元素，较直接利用海森矩阵求逆的方式减少了计算量。
(4)本方案通过增量更新雅克比矩阵和海森矩阵，递归确定协方差矩阵中的每个元素的方式，只是转换了确定方法，即采用增量递归的方法来确定各传感器参数的协方差矩阵，确定过程中并未做近似处理，所以在减少计算量的同时，保证了准确度。
实施例二
本公开实施例二提供一种用于传感器融合定位的协方差确定方法的具体实现，其流程如图4所示，包括如下步骤：
步骤s41：按设定规则从需要优化的传感器参数中确定目标参数。
传感器参数的迭代优化过程中，往往需要恢复的协方差矩阵(用于表征传感器参数的准确度和相关性)只是整体协方差矩阵的一部分，且针对不同的应用场景，具体需要恢复的协方差矩阵也不同，如实时定位系统，只需要恢复最新帧的位姿协方差矩阵；对于众包高精地图的信息无损传递，只需要恢复相关性强的最近几个关键帧的协方差矩阵；对于数据关联问题，只需要恢复某些特征点的协方差矩阵。
可以根据具体的应用场景确定出需要确定协方差的传感器参数，作为目标参数；参照图5所示，利用整体协方差矩阵，首先是传感器参数块的重新编排，例如需要确定协方差的传感器参数为xi、xj和xk；将待恢复协方差的传感器参数(目标参数)在协防差矩阵中的对应元素集中起来，形成待恢复的边缘协方差块，按照确定的边缘协方差块确定各待确定元素。
目标参数的确定，使得后续执行的步骤都在筛选出的目标参数的基础上执行，减小了计算量。
步骤s42：获取定位过程中当前迭代得到的变化步长大于设定步长阈值的目标参数为第一参数。
步骤s43：根据当前迭代得到的残差因子和参数编排步骤获得的第一参数，更新当前的雅克比矩阵。
步骤s44：根据更新后的雅克比矩阵中拆分的稠密子矩阵更新当前的海森矩阵。
参照图6所示，雅可比矩阵j中的每一行与其中一个残差因子res具有对应关系，j中的每一列与其中一个第一参数具有对应关系，具体的，每一行为一个残差因子关于各第一参数的一阶导数。
将j中的元素先按照行进行拆分，例如拆分出残差因子res_i对应的j_i、残差因子res_j对应的j_j……残差因子res_k对应的j_k；再针对拆分出的每一行进行稠密块(即稠密子矩阵)的拆分，例如j_i拆分出稠密块x_i、x_j……x_k；根据雅克比矩阵中拆分的稠密块更新当前的海森矩阵h。
执行完步骤s44后，需确定是否满足迭代结束条件，若是，执行步骤s45；若否，返回步骤s42，直至满足迭代结束条件。
步骤s45：输出最终的海森矩阵。
步骤s46：将海森矩阵进行cholesky分解得到上三角矩阵。
步骤s47：根据上三角矩阵中行列号最大的对角线元素的值，确定协方差矩阵中对应位置的元素的值。
步骤s48：按照行列号由大到小且先对角线元素后非对角线元素的顺序，通过递归的方式依次根据所述上三角矩阵中的相应元素的值和协方差矩阵中的相应元素的值，确定协方差矩阵中的未确定取值的元素的值。
按照行列号由大到小且先对角线元素后非对角线元素的顺序，具体的，以协方差矩阵(∑ij)nn为例，先确定协方差矩阵右下角的对角线元素∑nn；再按照列号由大到小的顺序确定第n行中的其他元素，按照行号由大到小的顺序确定第n列中的其他元素；再确定对角线元素∑(n-1)(n-1)；然后按照列号由大到小的顺序确定第n-1行中的其他元素，按照行号由大到小的顺序确定第n-1列中的其他元素；再确定对角线元素∑(n-2)(n-2)……直至协方差矩阵中的所有元素的值都确定完。
以确定完对角线元素∑ii后为例，具体先确定第i行中的其他元素或先确定第i列中的其他元素都可以，也可以同时进行。
具体的，协方差中元素的确定方法，可以包括：
协方差矩阵中的未确定取值的元素为对角线元素时，确定该元素的第一行号和第一列号，根据上三角矩阵中行号与所述第一行号一样的元素的值，和协方差矩阵中的行号大于第一行号且列号与第一列号一样的元素的值，确定该未确定取值的元素的值；
协方差矩阵中的未确定取值的元素为对非角线元素时，确定该元素的第二行号和第二列号，根据上三角矩阵中行号与第二行号一样的元素的值，协方差矩阵中的行号大于第二行号且列号与所述第二列号一样的元素的值，和协方差矩阵中的列号与第二列号一样且行号比第二行号大的元素的值，确定该未确定取值的元素的值。
具体的，可以通过下述公式分别确定协方差中的对角线和非对角线元素：
其中，uik或者是其他的下标，表示上三角交矩阵中的一个元素，下标表示其行号和列号。
具体的，上述协方差矩阵的确定过程中，针对中间缓存的各类矩阵，可以只保存矩阵中的稠密子矩阵及稠密子矩阵中的元素和原始矩阵中元素的位置对应关系，这样可以减少存储空间的需要，同时降低矩阵的空间复杂度。
基于本公开的发明构思，本公开实施例还提供一种传感器融合定位方法，包括：
根据按照上述方法确定的协方差矩阵中的对角线元素，确定对应的传感器参数的准确度；
将准确度大于设定阈值的参数作为下一步传感器融合定位中对应参数的初值。
基于本公开的发明构思，本公开实施例还提供一种用于传感器融合定位的协方差确定装置，其结构如图7所示，包括：
参数编排模块71，用于获取定位过程中当前迭代得到的变化步长大于设定步长阈值的传感器参数为第一参数；
雅克比矩阵解算模块72，用于根据当前迭代得到的残差因子和参数编排模块71获得的第一参数，更新当前的雅克比矩阵，所述残差因子是根据多传感器的测量数据和传感器参数确定的；
海森矩阵求解模块73，用于根据雅克比矩阵解算模块72得到的更新后的雅克比矩阵中拆分的稠密子矩阵更新当前的海森矩阵；参数编排模块71、雅克比矩阵解算模块72和海森矩阵求解模块73循环工作，直至迭代结束，海森矩阵求解模块73，还用于输出最终的海森矩阵；
分解模块74，用于将海森矩阵求解模块73输出的海森矩阵分解得到上三角矩阵；
协方差解算模块75，用于根据分解模块74得到的上三角矩阵通过递归的方式确定表征所述第一参数的准确度和相关性的协方差矩阵。
在一个实施例中，雅克比矩阵解算模块72，具体用于：
针对当前迭代得到的每个残差因子，根据该残差因子包含的第一参数，确定其在雅克比矩阵中对应元素的更新值；根据该残差因子的标识和其包含的第一参数的标识，确定该残差因子在雅克比矩阵中对应元素的位置；将当前的雅克比矩阵中所述位置的元素的值替换为相应的更新值。
在一个实施例中，海森矩阵求解模块73，具体用于：
针对更新后的雅克比矩阵中的每一行，从第一个非零元素开始，将连续的至少两个非零元素拆分为稠密子矩阵，根据所述稠密子矩阵确定更新子元素的值；根据更新子元素对应的第一参数的标识，确定该更新子元素在海森矩阵中对应元素的位置；将当前的海森矩阵中所述位置的元素的值替换为对应的更新子元素的值之和。
在一个实施例中，协方差解算模块75，具体用于：
根据所述上三角矩阵中行列号最大的对角线元素的值，确定协方差矩阵中对应位置的元素的值，所述协方差矩阵中元素的总行数和总列数分别与所述上三角矩阵中元素的总行数和总列数一致；按照行列号由大到小且先对角线元素后非对角线元素的顺序，通过递归的方式依次根据所述上三角矩阵中的相应元素的值和协方差矩阵中的相应元素的值，确定协方差矩阵中的未确定取值的元素的值。
在一个实施例中，协方差解算模块75，具体用于：
协方差矩阵中的未确定取值的元素为对角线元素时，确定该元素的第一行号和第一列号，根据上三角矩阵中行号与所述第一行号一样的元素的值，和协方差矩阵中的行号大于所述第一行号且列号与所述第一列号一样的元素的值，确定该未确定取值的元素的值；协方差矩阵中的未确定取值的元素为对非角线元素时，确定该元素的第二行号和第二列号，根据上三角矩阵中行号与所述第二行号一样的元素的值，协方差矩阵中的行号大于所述第二行号且列号与所述第二列号一样的元素的值，和协方差矩阵中的列号与所述第二列号一样且行号比所述第二行号大的元素的值，确定该未确定取值的元素的值。
在一个实施例中，上述装置还包括：目标参数确定模块76，用于：
按设定规则从需要优化的传感器参数中确定目标参数；
相应的，参数编排模块71，具体用于：
从目标参数确定模块76确定的目标参数中获取定位过程中当前迭代得到的变化步长大于设定步长阈值的目标参数为第一参数。
在一个实施例中，上述装置还包括：确定模块77，用于：
根据协方差解算模块75得到的协方差矩阵中的对角线元素，确定对应的传感器参数的准确度；和/或，根据协方差解算模块75得到的协方差矩阵中的元素，确定两两传感器参数的相关性。
关于上述实施例中的装置，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不做详细阐述说明。
基于本公开的发明构思，本公开实施例还提供一种计算机程序产品，包括计算机程序/指令，其中，该计算机程序/指令被处理器执行时实现上述用于传感器融合定位的协方差确定方法，或实现上述传感器融合定位方法。
基于本公开的发明构思，本公开实施例还提供一种计算机可读存储介质，其上存储有计算机指令，当该指令被处理器执行时实现上述用于传感器融合定位的协方差确定方法，或实现上述传感器融合定位方法。
基于本公开的发明构思，本公开实施例还提供一种服务器，包括：存储器、处理器及存储于存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现上述用于传感器融合定位的协方差确定方法，或实现上述传感器融合定位方法。
除非另外具体陈述，术语比如处理、计算、运算、确定、显示等等可以指一个或更多个处理或者计算系统、或类似设备的动作和/或过程，所述动作和/或过程将表示为处理系统的寄存器或存储器内的物理(如电子)量的数据操作和转换成为类似地表示为处理系统的存储器、寄存器或者其他此类信息存储、发射或者显示设备内的物理量的其他数据。信息和信号可以使用多种不同的技术和方法中的任何一种来表示。例如，在贯穿上面的描述中提及的数据、指令、命令、信息、信号、比特、符号和码片可以用电压、电流、电磁波、磁场或粒子、光场或粒子或者其任意组合来表示。
应该明白，公开的过程中的步骤的特定顺序或层次是示例性方法的实例。基于设计偏好，应该理解，过程中的步骤的特定顺序或层次可以在不脱离本公开的保护范围的情况下得到重新安排。所附的方法权利要求以示例性的顺序给出了各种步骤的要素，并且不是要限于所述的特定顺序或层次。
在上述的详细描述中，各种特征一起组合在单个的实施方案中，以简化本公开。不应该将这种公开方法解释为反映了这样的意图，即，所要求保护的主题的实施方案需要清楚地在每个权利要求中所陈述的特征更多的特征。相反，如所附的权利要求书所反映的那样，本公开处于比所公开的单个实施方案的全部特征少的状态。因此，所附的权利要求书特此清楚地被并入详细描述中，其中每项权利要求独自作为本公开单独的优选实施方案。
本领域技术人员还应当理解，结合本文的实施例描述的各种说明性的逻辑框、模块、电路和算法步骤均可以实现成电子硬件、计算机软件或其组合。为了清楚地说明硬件和软件之间的可交换性，上面对各种说明性的部件、框、模块、电路和步骤均围绕其功能进行了一般地描述。至于这种功能是实现成硬件还是实现成软件，取决于特定的应用和对整个系统所施加的设计约束条件。熟练的技术人员可以针对每个特定应用，以变通的方式实现所描述的功能，但是，这种实现决策不应解释为背离本公开的保护范围。
结合本文的实施例所描述的方法或者算法的步骤可直接体现为硬件、由处理器执行的软件模块或其组合。软件模块可以位于ram存储器、闪存、rom存储器、eprom存储器、eeprom存储器、寄存器、硬盘、移动磁盘、cd-rom或者本领域熟知的任何其它形式的存储介质中。一种示例性的存储介质连接至处理器，从而使处理器能够从该存储介质读取信息，且可向该存储介质写入信息。当然，存储介质也可以是处理器的组成部分。处理器和存储介质可以位于asic中。该asic可以位于用户终端中。当然，处理器和存储介质也可以作为分立组件存在于用户终端中。
对于软件实现，本申请中描述的技术可用执行本申请所述功能的模块(例如，过程、函数等)来实现。这些软件代码可以存储在存储器单元并由处理器执行。存储器单元可以实现在处理器内，也可以实现在处理器外，在后一种情况下，它经由各种手段以通信方式耦合到处理器，这些都是本领域中所公知的。
上文的描述包括一个或多个实施例的举例。当然，为了描述上述实施例而描述部件或方法的所有可能的结合是不可能的，但是本领域普通技术人员应该认识到，各个实施例可以做进一步的组合和排列。因此，本文中描述的实施例旨在涵盖落入所附权利要求书的保护范围内的所有这样的改变、修改和变型。此外，就说明书或权利要求书中使用的术语“包含”，该词的涵盖方式类似于术语“包括”，就如同“包括，”在权利要求中用作衔接词所解释的那样。此外，使用在权利要求书的说明书中的任何一个术语“或者”是要表示“非排它性的或者”。术语“第一”、“第二”、“第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。
- 还没有人留言评论。精彩留言会获得点赞！