CN112765548A - Covariance determination method, positioning method and device for sensor fusion positioning - Google Patents
Covariance determination method, positioning method and device for sensor fusion positioning Download PDFInfo
- Publication number
- CN112765548A CN112765548A CN202110042412.1A CN202110042412A CN112765548A CN 112765548 A CN112765548 A CN 112765548A CN 202110042412 A CN202110042412 A CN 202110042412A CN 112765548 A CN112765548 A CN 112765548A
- Authority
- CN
- China
- Prior art keywords
- matrix
- parameter
- covariance
- determining
- sensor
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 101
- 230000004927 fusion Effects 0.000 title claims abstract description 41
- 239000011159 matrix material Substances 0.000 claims abstract description 335
- 230000008569 process Effects 0.000 claims abstract description 30
- 230000008859 change Effects 0.000 claims abstract description 21
- 238000005259 measurement Methods 0.000 claims abstract description 11
- 238000004364 calculation method Methods 0.000 claims abstract description 10
- 238000000354 decomposition reaction Methods 0.000 claims description 12
- 238000004590 computer program Methods 0.000 claims description 10
- 230000004807 localization Effects 0.000 claims 4
- 230000015654 memory Effects 0.000 description 11
- 238000005457 optimization Methods 0.000 description 9
- 238000012545 processing Methods 0.000 description 7
- 230000006870 function Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 239000002245 particle Substances 0.000 description 2
- 238000012216 screening Methods 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 230000004075 alteration Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Pure & Applied Mathematics (AREA)
- Mathematical Optimization (AREA)
- Mathematical Analysis (AREA)
- Data Mining & Analysis (AREA)
- Theoretical Computer Science (AREA)
- Computational Mathematics (AREA)
- Automation & Control Theory (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Computing Systems (AREA)
- Radar Systems Or Details Thereof (AREA)
Abstract
The disclosure discloses a covariance determination method, a positioning method and a device for sensor fusion positioning. The covariance determination method comprises the following steps: acquiring a sensor parameter with a change step length larger than a set step length threshold value obtained by current iteration in a positioning process as a first parameter; updating a current Jacobian matrix according to the first parameter and a residual factor obtained by current iteration, wherein the residual factor is determined according to the measurement data of the sensor and the sensor parameter; updating the current Hessian matrix according to the split dense sub-matrix in the updated Jacobian matrix, returning to execute the step of obtaining the first parameter until the iteration end condition is met, and outputting the final Hessian matrix; decomposing the Hessian matrix to obtain an upper triangular matrix; and determining a covariance matrix representing the accuracy and the correlation of the first parameter in a recursive manner according to the upper triangular matrix. The accuracy of the determined covariance can be ensured on the basis of reducing the amount of calculation.
Description
Technical Field
The present disclosure relates to the field of positioning technologies, and in particular, to a covariance determination method, a positioning method, and an apparatus for sensor fusion positioning.
Background
The multi-sensor fusion positioning generally refers to a positioning System including a vision sensor (such as a camera), an Inertial Measurement Unit (IMU, abbreviated as "Inertial Navigation") and a Global Navigation Satellite System (GNSS), and the System has the advantages of high positioning accuracy and low cost, and becomes a positioning scheme mainly selected for an application scenario requiring a high-accuracy (centimeter-level) positioning result. In the process of obtaining a high-precision positioning result through sensor fusion positioning, parameters such as the position and the attitude of each sensor need to be determined, and the covariance of the parameters needs to be determined, so that the positioning precision radius of the fusion positioning can be estimated, the correlation can be evaluated, and the like.
In the prior art, covariance estimation is generally performed by adopting a hessian matrix inversion method, but the number of dimensions of parameters of multi-sensor fusion positioning is large, the computational power consumption of direct matrix inversion is huge, the system cost is increased, and in order to reduce the cost, approximation processing can influence the accuracy of estimated covariance. Therefore, there is a need to provide new covariance determination schemes to meet the requirements of cost and accuracy.
Disclosure of Invention
In view of the above, the present disclosure is proposed in order to provide a covariance determination method, a positioning method and an apparatus for sensor fusion positioning that overcome or at least partially solve the above problems.
In a first aspect, an embodiment of the present disclosure provides a covariance determination method for sensor fusion positioning, including:
a parameter arranging step, comprising: acquiring a sensor parameter with a change step length larger than a set step length threshold value obtained by current iteration in a positioning process as a first parameter;
the Jacobian matrix resolving step comprises the following steps: updating a current Jacobian matrix according to a residual factor obtained by current iteration and a first parameter obtained in the parameter arrangement step, wherein the residual factor is determined according to measurement data of a sensor and sensor parameters;
the Hessian matrix solving step comprises the following steps: updating the current Hessian matrix according to the split dense sub-matrix in the updated Jacobian matrix, returning to the parameter arrangement step until an iteration end condition is met, and outputting a final Hessian matrix;
a decomposition step comprising: decomposing the Hessian matrix to obtain an upper triangular matrix;
a covariance solution step including: and determining a covariance matrix representing the accuracy and the correlation of the first parameter in a recursive mode according to the upper triangular matrix.
In a second aspect, an embodiment of the present disclosure provides a sensor fusion positioning method, including:
determining the accuracy of the corresponding sensor parameter according to the diagonal elements in the covariance matrix determined by the method;
and taking the parameter with the accuracy greater than the set threshold value as an initial value of the corresponding parameter in the next sensor fusion positioning.
In a third aspect, an embodiment of the present disclosure provides a covariance determination apparatus for sensor fusion positioning, including:
the parameter arranging module is used for acquiring a sensor parameter of which the change step length obtained by current iteration in the positioning process is larger than a set step length threshold value as a first parameter;
the Jacobian matrix resolving module is used for updating a current Jacobian matrix according to a residual factor obtained by current iteration and a first parameter obtained by the parameter arranging module, wherein the residual factor is determined according to measurement data of a plurality of sensors and sensor parameters;
the hessian matrix solving module is used for updating the current hessian matrix according to the split dense sub-matrix in the updated Jacobian matrix obtained by the Jacobian matrix solving module; the parameter arranging module, the Jacobian matrix solving module and the Hessian matrix solving module work circularly until iteration is finished, and the Hessian matrix solving module is also used for outputting a final Hessian matrix;
the decomposition module is used for decomposing the Hessian matrix output by the Hessian matrix solving module to obtain an upper triangular matrix;
a covariance calculation module for determining a covariance matrix representing the accuracy and correlation of the first parameter in a recursive manner according to the upper triangular matrix obtained by the decomposition module
In a fourth aspect, the present disclosure provides a computer program product comprising a computer program/instructions, wherein the computer program/instructions, when executed by a processor, implement the above covariance determination method for sensor fusion positioning, or implement the above sensor fusion positioning method.
The covariance determination method for sensor fusion positioning provided by the embodiment of the disclosure comprises the following steps: a parameter arrangement step, wherein a sensor parameter with a change step length larger than a set step length threshold value obtained by current iteration in the positioning process is obtained as a first parameter; a Jacobian matrix calculating step, namely updating the current Jacobian matrix according to a residual factor obtained by current iteration and the first parameter obtained in the parameter arranging step, wherein the residual factor is determined according to the measurement data of the multiple sensors and the sensor parameters; a hessian matrix solving step, namely updating the current hessian matrix according to the split dense sub-matrix in the updated Jacobian matrix, and returning to the parameter arranging step until iteration is finished to obtain a final hessian matrix; a decomposition step, decomposing the Hessian matrix to obtain an upper triangular matrix; and a covariance calculation step of determining a covariance matrix representing the accuracy and the correlation of the first parameter in a recursive manner according to the upper triangular matrix. The beneficial effects of the above technical scheme at least include:
(1) and screening the parameter with the change step length larger than a set step length threshold value as a first parameter according to the change step length of the sensor parameter obtained by the current iteration, and updating the current Jacobian matrix according to the first parameter and the residual factor obtained by the current iteration. After each iteration is finished, only the element corresponding to the sensor parameter with larger change step length is updated by using the updating result of the Jacobian matrix after the last iteration is finished, so that the calculated amount is reduced; the current Hessian matrix is updated according to the split dense sub-matrix in the Jacobian matrix, the calculated amount of a sparse block is saved, and compared with a mode of solving a second derivative for a residual factor or a mode of determining the Hessian matrix directly according to the product of the Jacobian matrix and the transpose thereof, the calculated amount is further reduced; the Hessian matrix is decomposed to obtain an upper triangular matrix, each element in the covariance matrix is sequentially determined in a recursion mode, and the calculated amount is reduced compared with a mode of directly utilizing Hessian matrix inversion.
(2) According to the method, the Jacobian matrix and the Hessian matrix are updated in an incremental mode, each element in the covariance matrix is determined in a recursion mode, only the determination method is converted, namely the covariance matrix of each sensor parameter is determined by adopting the incremental recursion method, and approximate processing is not performed in the determination process, so that the accuracy is guaranteed while the calculated amount is reduced.
Additional features and advantages of the disclosure will be set forth in the description which follows, and in part will be obvious from the description, or may be learned by the practice of the disclosure. The objectives and other advantages of the disclosure may be realized and attained by the structure particularly pointed out in the written description and claims hereof as well as the appended drawings.
The technical solution of the present disclosure is further described in detail by the accompanying drawings and examples.
Drawings
The accompanying drawings are included to provide a further understanding of the disclosure and are incorporated in and constitute a part of this specification, illustrate embodiments of the disclosure and together with the description serve to explain the principles of the disclosure and not to limit the disclosure. In the drawings:
FIG. 1 is a flowchart of a covariance determination method for sensor fusion positioning according to an embodiment of the disclosure;
FIG. 2 is a flowchart illustrating an implementation of step S12 in FIG. 1;
FIG. 3 is a flowchart illustrating an implementation of step S13 in FIG. 1;
FIG. 4 is a flowchart illustrating a specific implementation of a covariance determination method for sensor fusion positioning according to a second embodiment of the disclosure;
FIG. 5 is a diagram illustrating an exemplary arrangement of a sensor parameter block according to an embodiment of the present disclosure;
FIG. 6 is an exemplary diagram of determining a Hessian matrix from Jacobian matrix increments in an embodiment of the disclosure;
fig. 7 is a schematic structural diagram of a covariance determination apparatus for sensor fusion positioning in an embodiment of the present disclosure.
Detailed Description
Exemplary embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. While exemplary embodiments of the present disclosure are shown in the drawings, it should be understood that the present disclosure may be embodied in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the scope of the disclosure to those skilled in the art.
In order to solve the problems of high computational power consumption and low precision in the covariance determination process of positioning parameters of a multi-sensor fusion positioning system in the prior art, embodiments of the present disclosure provide a covariance determination method, a positioning method, and a device for sensor fusion positioning, which can more accurately recover covariance, and the computational complexity in the whole process is relatively small.
The multi-sensor fusion positioning system in the following embodiments generally includes a vision sensor, an inertial navigation system, and a global navigation satellite system, and determines sensor parameters, that is, positioning parameters related to a sensor, which needs to be optimized in a positioning process, or simply referred to as positioning parameters, by using multi-sensor positioning in an iterative optimization manner according to measurement data of the multi-sensor. Specifically, the sensor parameters may be direct pose parameters, or may be other parameters to be optimized in the process of determining the pose parameters, for example, the sensor parameters iteratively optimized during initialization include: the pose of the vision sensor (the position and the attitude of each frame relative to the first frame), the scale of the vision sensor, the speed of the vision sensor, the rotating external parameters between a camera coordinate system and a world coordinate system, the zero offset of a gyroscope and the like; the sensor parameters for iterative optimization in the positioning process comprise: the system comprises a relative world coordinate system, an absolute world coordinate system, a relative position between an antenna of a global positioning system and inertial navigation, and the like. In the iterative optimization process, a plurality of residual factors including measurement data, namely observation data, of each sensor and sensor parameters to be optimized need to be established, and each residual factor may include part of the observation data and part of the sensor parameters.
Sensor parameter X ═ X1,X2,……，Xn)TFor an n-dimensional parameter, the covariance matrix is:
therein, sigmaijThe covariance matrix is the covariance between the ith sensor parameter and the jth sensor parameter, i.e. the elements in the ith row and jth column in the covariance matrix, i is 1,2 … … n, and j is 1,2 … … n.
The determining of the covariance Matrix of the sensor parameters may be determining a Jacobian Matrix according to the sensor parameters and the residual factors, determining a Hessian Matrix according to the Jacobian Matrix, and determining the covariance Matrix according to the Hessian Matrix.
In the vector calculus, the jacobian matrix is a matrix in which the first-order partial derivatives are arranged in a certain manner, and the determinant thereof is called jacobian. The significance of the jacobian matrix is that it embodies a relatively good linear approximation of a given point to a differentiable equation.
Suppose there are m residual factors r1,r2,……，rmN sensor parameters X1,X2,……，XnThen, the determined jacobian matrix J is:
wherein, JtiRepresenting the elements of the ith row and column in the Jacobian matrix, i.e. the tth residual factor rtWith respect to the ith sensor parameter xiT 1,2 … … m, i 1,2 … … n.
The hessian matrix is a square matrix formed by second partial derivatives of a multivariate function and describes the local curvature of the function. Target factor formed by assumed residual factorSub (i.e. the sum of the residual factors) is r, n sensor parameters X1,X2,……，XnThen, the determined hessian matrix H is:
wherein HijThe second partial derivatives of the target factor r with respect to the ith and jth sensor parameters, i 1,2 … … n, and j 1,2 … … n, are shown.
The method for determining the covariance of the specific sensor parameters is described in detail in the following embodiments.
Example one
An embodiment of the present disclosure provides a covariance determination method for sensor fusion positioning, a flow of which is shown in fig. 1, and the method includes the following steps:
step S11: and (5) arranging parameters.
The method specifically comprises the following steps: and acquiring a sensor parameter with a change step length larger than a set step length threshold value obtained by current iteration in the positioning process as a first parameter.
In the positioning process, iterative optimization needs to be carried out on each sensor parameter, only a small part of parameters possibly change in each iterative optimization process of the sensor parameters, which means that a lot of parameters do not need to be linearized again, so that corresponding residual factors of the sensor parameters with small step change in iterative optimization do not need to be linearized again, values of corresponding elements of the sensor parameters in a Jacobian matrix do not need to be updated, and the calculation power consumption can be greatly reduced.
Step S12: and (5) resolving a Jacobian matrix.
The method specifically comprises the following steps: and updating the current Jacobian matrix according to a residual factor obtained by the current iteration and the first parameter obtained in the parameter arrangement step, wherein the residual factor is determined according to the measurement data of the sensor and the sensor parameter.
In one embodiment, as shown with reference to FIG. 2, the update of the Jacobian matrix may include the steps of:
step S121: and determining the update value of the corresponding element of each residual factor obtained by the current iteration in the Jacobian matrix according to the first parameter contained in the residual factor.
Each residual factor may include one or more first parameters, and for each residual factor, according to the iteration result of the first parameters included in the residual factor at the current iteration, a first derivative of the residual factor with respect to the first parameters is respectively determined as an updated value of the corresponding element of the residual factor and each first parameter included in the residual factor in the jacobian matrix. Specifically, a residual factor and a first parameter contained therein correspond to an element in the jacobian matrix.
Step S122: and determining the position of the corresponding element of the residual factor in the Jacobian matrix according to the identification of the residual factor and the identification of the first parameter contained in the residual factor.
Step S123: and replacing the values of the elements of the position in the current Jacobian matrix with the corresponding updated values.
E.g. residual factor r2Including a first parameter x1And x5Then r can be determined2With respect to x1As a residual factor r2And a first parameter x1Determining r corresponding to the updated value of element A in the Jacobian matrix2With respect to x5As a residual factor r2And a first parameter x5The updated value of the corresponding element B in the Jacobian matrix; according to the identity (subscript 2) of the residual factor and the first parameter x contained therein1Is determined to be the 2 nd row and 1 st column of the element a in the jacobian matrix described below, according to the identity of the residual factor (subscript 2) and the first parameter x contained therein5Is used to determine the position of element B in the jacobian matrix described below as row 2, column 5. Replacing the value of the element of the 2 nd row and the 1 st column in the current Jacobian matrix with the updated value of the element A, and replacing the value of the element of the 2 nd row and the 5 th column in the current Jacobian matrix with the updated value of the element B; because of the residual factor r2No other residual factors are included, so the other elements in row 2 of the jacobian matrix are unchanged.
Wherein, JtiRepresenting the elements of the ith row and column in the Jacobian matrix, i.e. the tth residual factor rtWith respect to the ith sensor parameter xiT 1,2 … … m, i 1,2 … … n.
Step S13: and (5) solving the hessian matrix.
The method specifically comprises the following steps: and updating the current Hessian matrix according to the split dense sub-matrix in the updated Jacobian matrix.
The product of the Jacobian matrix and the transposed matrix of the Jacobian matrix can be used as the Hessian matrix, but the complexity of the method is high, for example, m-dimensional residual factors, and when n-dimensional sensor parameters are used, the overall complexity is (mn)2. Because each residual factor usually only contains few sensor parameters, many elements in the Jacobian matrix are all 0, namely the Jacobian matrix is sparse, and therefore the quick Hessian matrix can be updated in an incremental mode.
Specifically, the dense sub-matrix refers to a matrix with more non-0 elements.
In one embodiment, referring to fig. 3, updating the current hessian matrix according to the split dense sub-matrices in the updated jacobian matrix may include the following steps:
step S131: for each row in the updated Jacobian matrix, starting from the first non-zero element, splitting at least two continuous non-zero elements into a dense sub-matrix, and determining the value of the updated sub-element according to the dense sub-matrix.
Specifically, for each row in the jacobian matrix, one dense sub-matrix may be split, or multiple dense sub-matrices may be split, for example, starting from a first non-zero element, at intervals of zero element, at least two consecutive non-zero elements are split into one dense sub-matrix, and if a next element of one non-zero element is zero element, the non-zero element and subsequent consecutive non-zero elements are split into one dense sub-matrix.
The value of the update sub-element is determined according to the dense sub-matrix, which may be the product of the dense sub-matrix and the dense sub-matrix transpose, and the non-zero element is determined as the value of the update sub-element.
Step S132: and determining the position of the corresponding element of the updated sub-element in the Hessian matrix according to the identifier of the first parameter corresponding to the updated sub-element.
Specifically, the value of each update sub-element is obtained according to the values of two elements in the jacobian matrix, which may be two identical elements in the jacobian matrix or two different elements; the elements in the Jacobian matrix correspond to the unique first parameter identifiers, and the positions of the corresponding elements in the Hessian matrix of the updated sub-elements are determined according to the two first parameter identifiers.
For example, the dense sub-matrix A is
AATIs composed of
The dense sub-matrix a may determine 4 second update sub-elements
And
the corresponding positions of the corresponding elements in the hessian matrix are respectively the 1 st row, the 5 th column, the 5 th row, the 1 st column and the 5 th row, the 5 th column.
Step S133: and replacing the value of the element at the position in the current Hessian matrix with the sum of the values of the corresponding updating sub-elements.
Decomposing elements in the Jacobian matrix corresponding to each residual error factor into a plurality of dense incremental blocks, and updating a plurality of incremental dense sub-matrixes on the sparse Hessian matrix; and in combination with the stable parameters in the step S11, that is, the first parameter with small step change is not updated in the corresponding element in the jacobian matrix, and the jacobian matrix and the hessian matrix are updated incrementally, so that the covariance determination efficiency is improved.
The fusion positioning is to determine sensor parameters by an iterative optimization method, the current Jacobian matrix and Hessian matrix are required to be updated after each iteration of one-time optimization, and the covariance matrix is recovered according to the final Hessian matrix after the iteration is finished. Therefore, after step S13 is completed, it is determined whether the iteration end condition is satisfied, and if so, step S14 is performed; if not, the process returns to step S11 until the iteration end condition is satisfied.
Step S14: and outputting the final Hessian matrix.
Step S15: and (5) carrying out hessian matrix decomposition.
The method specifically comprises the following steps: and decomposing the Hessian matrix to obtain an upper triangular matrix.
In one embodiment, the hessian matrix may be subjected to Cholesky decomposition to obtain an upper triangular matrix. And decomposing the Hessian matrix into a product of an upper triangular matrix and a lower triangular matrix to obtain the upper triangular matrix.
Step S16: and (5) covariance calculation.
The method specifically comprises the step of determining a covariance matrix representing the accuracy and the correlation of the first parameter in a recursive mode according to the upper triangular matrix.
The covariance matrix is determined by utilizing a hessian matrix inversion method, and the values of elements in the covariance matrix can be determined step by step in a recursive method because the number of dimensions of the first parameter is large and large computational power consumption is caused.
In an embodiment, the values of the elements with undetermined dereferencing in the covariance matrix may be determined in sequence from large to small in the order of the row and column numbers, and according to the values of the corresponding elements in the upper triangular matrix and the values of the corresponding elements with determined dereferencing in the covariance matrix in a recursive manner. The total number of rows and the total number of columns of the elements in the covariance matrix are respectively consistent with the total number of rows and the total number of columns of the elements in the upper triangular matrix.
Specifically, first, according to the value of the diagonal element with the largest row and column number in the upper triangular matrix, the value of the element at the corresponding position in the covariance matrix is determined, that is, according to the value of the diagonal element with the largest row and column number in the upper triangular matrix, the value of the element with the corresponding position in the covariance matrix as the determined value is determined for the first time; and determining the values of the elements with undetermined values in the covariance matrix according to the sequence of the row and column numbers from large to small, the diagonal elements first and the off-diagonal elements sequentially in a recursion mode according to the values of the corresponding elements in the upper triangular matrix and the values of the corresponding elements in the covariance matrix. And finally, determining the value of each element in the covariance matrix, and updating the covariance matrix.
Specifically, the diagonal elements of the covariance matrix represent the accuracy of the first parameter, that is, the precision radius of the first element, so that the accuracy of the corresponding sensor parameter can be determined according to the diagonal elements in the covariance matrix; each element in the covariance matrix represents the correlation of two corresponding sensor parameters, so that the correlation of every two sensor parameters can be determined according to the elements in the covariance matrix.
The technical scheme in the first embodiment has the beneficial effects that:
(1) and screening the sensor parameters with the change step length larger than a set step length threshold value as first parameters according to the change step length of the sensor parameters obtained by the current iteration, and updating the current Jacobian matrix according to the first parameters and the residual factors obtained by the current iteration. After each iteration is finished, only the element corresponding to the sensor parameter with the larger change step length is updated by using the updating result of the Jacobian matrix after the last iteration is finished, so that the calculated amount is reduced.
(2) The current Hessian matrix is updated according to the split dense sub-matrix in the Jacobian matrix, the calculation amount of a sparse block is saved, and the calculation amount is further reduced compared with a mode of solving a second derivative for a residual factor or a mode of determining the Hessian matrix directly according to the product of the Jacobian matrix and the transpose thereof.
(3) The Hessian matrix is decomposed to obtain an upper triangular matrix, each element in the covariance matrix is sequentially determined in a recursion mode, and the calculated amount is reduced compared with a mode of directly utilizing Hessian matrix inversion.
(4) According to the method, the Jacobian matrix and the Hessian matrix are updated in an incremental mode, each element in the covariance matrix is determined in a recursion mode, only the determination method is converted, namely the covariance matrix of each sensor parameter is determined by adopting the incremental recursion method, and approximate processing is not performed in the determination process, so that the accuracy is guaranteed while the calculated amount is reduced.
Example two
The second embodiment of the present disclosure provides a specific implementation of a covariance determination method for sensor fusion positioning, where a flow is shown in fig. 4, and the method includes the following steps:
step S41: and determining target parameters from the sensor parameters needing to be optimized according to set rules.
In the iterative optimization process of the sensor parameters, the covariance matrix (used for representing the accuracy and the correlation of the sensor parameters) which is often required to be restored is only a part of the overall covariance matrix, and the covariance matrix which is particularly required to be restored is different for different application scenes, so that as a real-time positioning system, only the pose covariance matrix of the latest frame needs to be restored; for the information lossless transmission of the crowdsourcing high-precision map, only the covariance matrixes of the latest key frames with strong correlation are needed to be recovered; for the data correlation problem, only the covariance matrix of some feature points needs to be recovered.
The sensor parameters of the covariance to be determined can be determined according to a specific application scene and serve as target parameters; referring to fig. 5, using the global covariance matrix, firstly, rearranging the sensor parameter blocks, for example, the sensor parameters xi, xj, and xk required to determine the covariance; corresponding elements of sensor parameters (target parameters) of covariance to be recovered in a covariance matrix are gathered to form an edge covariance block to be recovered, and each element to be determined is determined according to the determined edge covariance block.
And the target parameters are determined, so that the subsequent steps are executed on the basis of the screened target parameters, and the calculation amount is reduced.
Step S42: and acquiring a target parameter of which the change step length obtained by current iteration in the positioning process is larger than a set step length threshold value as a first parameter.
Step S43: and updating the current Jacobian matrix according to the residual factors obtained by the current iteration and the first parameters obtained in the parameter arrangement step.
Step S44: and updating the current Hessian matrix according to the split dense sub-matrix in the updated Jacobian matrix.
Referring to fig. 6, each row in the jacobian matrix J corresponds to one of the residual factors Res, and each column in J corresponds to one of the first parameters, specifically, each row is a first derivative of one of the residual factors with respect to each of the first parameters.
Splitting elements in J according to lines, for example, splitting the elements into J _ i corresponding to residual factors Res _ i and J _ J … … corresponding to residual factors Res _ k; splitting dense blocks (namely dense sub-matrixes) for each split row, for example, splitting J _ i into dense blocks X _ i and X _ J … … X _ k; the current hessian matrix H is updated from the split dense chunks in the jacobian matrix.
After step S44 is completed, it is determined whether an iteration end condition is satisfied, and if so, step S45 is performed; if not, the process returns to step S42 until the iteration end condition is satisfied.
Step S45: and outputting the final Hessian matrix.
Step S46: and performing Cholesky decomposition on the Hessian matrix to obtain an upper triangular matrix.
Step S47: and determining the value of the element at the corresponding position in the covariance matrix according to the value of the diagonal element with the maximum row number and column number in the upper triangular matrix.
Step S48: and determining the values of the elements with undetermined values in the covariance matrix according to the sequence of the row and column numbers from large to small, the diagonal elements first and the off-diagonal elements sequentially in a recursion mode according to the values of the corresponding elements in the upper triangular matrix and the values of the corresponding elements in the covariance matrix.
According to the sequence of the row-column numbers from large to small, the diagonal elements first and the off-diagonal elements later, specifically, the covariance matrix (sigma)ij)nnFor example, first, the determinationDiagonal element Σ in the lower right corner of the covariance matrixnn(ii) a Determining other elements in the nth row according to the sequence of the column numbers from large to small, and determining other elements in the nth column according to the sequence of the row numbers from large to small; then determining diagonal element sigma(n-1)(n-1)(ii) a Then determining other elements in the n-1 th row according to the sequence of the column numbers from large to small, and determining other elements in the n-1 th column according to the sequence of the row numbers from large to small; then determining diagonal element sigma(n-2)(n-2)… … until the values of all elements in the covariance matrix have been determined.
To determine the complete diagonal element sigmaiiFor example, the other elements in the ith row or the ith column may be determined first, or may be determined simultaneously.
Specifically, the method for determining elements in the covariance may include:
when the element of undetermined dereferencing in the covariance matrix is a diagonal element, determining a first row number and a first column number of the element, and determining the value of the element of undetermined dereferencing according to the value of the element of which the row number is the same as the first row number in the upper triangular matrix and the value of the element of which the row number is greater than the first row number and the column number is the same as the first column number in the covariance matrix;
and when the element with the undetermined value in the covariance matrix is a diagonal line element, determining a second row number and a second column number of the element, and determining the value of the element with the undetermined value according to the value of the element with the row number being the same as the second row number in the upper triangular matrix, the value of the element with the row number being larger than the second row number and the column number being the same as the second column number in the covariance matrix, and the value of the element with the column number being the same as the second column number and the row number being larger than the second row number in the covariance matrix.
Specifically, the diagonal and off-diagonal elements in the covariance may be determined by the following equations:
wherein, UikOr other subscript, which indicates an element of the upper triangular cross matrix and indicates its row and column number.
Specifically, in the process of determining the covariance matrix, for each type of matrix cached in the middle, only the dense sub-matrix in the matrix and the corresponding relationship between the elements in the dense sub-matrix and the elements in the original matrix may be stored, so that the requirement for a storage space may be reduced, and the spatial complexity of the matrix may be reduced.
Based on the inventive concept of the present disclosure, an embodiment of the present disclosure further provides a sensor fusion positioning method, including:
determining the accuracy of the corresponding sensor parameter according to the diagonal elements in the covariance matrix determined by the method;
and taking the parameter with the accuracy greater than the set threshold value as an initial value of the corresponding parameter in the next sensor fusion positioning.
Based on the inventive concept of the present disclosure, an embodiment of the present disclosure further provides a covariance determination apparatus for sensor fusion positioning, which is structurally shown in fig. 7 and includes:
the parameter arranging module 71 is configured to acquire a sensor parameter, which is obtained by current iteration in the positioning process and has a change step length larger than a set step length threshold, as a first parameter;
the Jacobian matrix calculating module 72 is used for updating the current Jacobian matrix according to the residual factors obtained by the current iteration and the first parameters obtained by the parameter arranging module 71, wherein the residual factors are determined according to the measurement data of the multiple sensors and the sensor parameters;
the hessian matrix solving module 73 is used for updating the current hessian matrix according to the dense sub-matrix split in the updated Jacobian matrix obtained by the Jacobian matrix solving module 72; the parameter arranging module 71, the Jacobian matrix solving module 72 and the Hessian matrix solving module 73 circularly work until the iteration is finished, and the Hessian matrix solving module 73 is also used for outputting a final Hessian matrix;
the decomposition module 74 is configured to decompose the hessian matrix output by the hessian matrix solving module 73 to obtain an upper triangular matrix;
and a covariance solution module 75, configured to determine a covariance matrix representing the accuracy and the correlation of the first parameter in a recursive manner according to the upper triangular matrix obtained by the decomposition module 74.
In one embodiment, the Jacobian matrix solution module 72 is specifically configured to:
aiming at each residual error factor obtained by current iteration, determining an updated value of a corresponding element of the residual error factor in a Jacobian matrix according to a first parameter contained in the residual error factor; determining the position of the corresponding element of the residual factor in the Jacobian matrix according to the identifier of the residual factor and the identifier of the first parameter contained in the residual factor; and replacing the values of the elements of the position in the current Jacobian matrix with the corresponding updated values.
In one embodiment, the hessian matrix solving module 73 is specifically configured to:
for each row in the updated Jacobian matrix, starting from a first non-zero element, splitting at least two continuous non-zero elements into a dense sub-matrix, and determining the value of an updated sub-element according to the dense sub-matrix; determining the position of the corresponding element of the updated sub-element in the Hessian matrix according to the identifier of the first parameter corresponding to the updated sub-element; and replacing the value of the element of the position in the current Hessian matrix with the sum of the values of the corresponding updating sub-elements.
In one embodiment, the covariance solution module 75 is specifically configured to:
determining the value of an element at a corresponding position in a covariance matrix according to the value of a diagonal element with the maximum row number and the maximum column number in the upper triangular matrix, wherein the total row number and the total column number of the element in the covariance matrix are respectively consistent with the total row number and the total column number of the element in the upper triangular matrix; and determining the values of the elements with undetermined values in the covariance matrix according to the sequence of the row and column numbers from large to small, the diagonal elements first and the off-diagonal elements sequentially in a recursion mode according to the values of the corresponding elements in the upper triangular matrix and the values of the corresponding elements in the covariance matrix.
In one embodiment, the covariance solution module 75 is specifically configured to:
when the element of undetermined dereferencing in the covariance matrix is a diagonal element, determining a first row number and a first column number of the element, and determining the value of the element of undetermined dereferencing according to the value of the element of which the row number is the same as the first row number in the upper triangular matrix and the value of the element of which the row number is greater than the first row number and the column number is the same as the first column number in the covariance matrix; and when the element with the undetermined dereferencing in the covariance matrix is a diagonal non-angular line element, determining a second row number and a second column number of the element, and according to the values of the elements with the row numbers same as the second row number in the upper triangular matrix, determining the value of the element with the undetermined dereferencing according to the values of the elements with the row numbers larger than the second row number and the column numbers same as the second column number in the covariance matrix and the values of the elements with the column numbers same as the second column number and the row numbers larger than the second row number in the covariance matrix.
In one embodiment, the above apparatus further comprises: a target parameter determination module 76 for:
determining target parameters from sensor parameters needing to be optimized according to a set rule;
correspondingly, the parameter arrangement module 71 is specifically configured to:
the target parameter with the change step length larger than the set step length threshold obtained from the current iteration in the positioning process is obtained from the target parameters determined by the target parameter determination module 76 as the first parameter.
In one embodiment, the above apparatus further comprises: a determining module 77 configured to:
determining the accuracy of the corresponding sensor parameter according to the diagonal elements in the covariance matrix obtained by the covariance calculation module 75; and/or determining the correlation between the two sensor parameters according to the elements in the covariance matrix obtained by the covariance solution module 75.
With regard to the apparatus in the above-described embodiment, the specific manner in which each module performs the operation has been described in detail in the embodiment related to the method, and will not be elaborated here.
Based on the inventive concept of the present disclosure, embodiments of the present disclosure also provide a computer program product, which includes a computer program/instruction, where the computer program/instruction, when executed by a processor, implements the above covariance determination method for sensor fusion positioning, or implements the above sensor fusion positioning method.
Based on the inventive concept of the present disclosure, embodiments of the present disclosure also provide a computer-readable storage medium, on which computer instructions are stored, and when the instructions are executed by a processor, the covariance determination method for sensor fusion positioning described above is implemented, or the sensor fusion positioning method described above is implemented.
Based on the inventive concept of the present disclosure, an embodiment of the present disclosure further provides a server, including: the sensor fusion positioning system comprises a memory, a processor and a computer program which is stored on the memory and can run on the processor, wherein the processor realizes the covariance determination method for sensor fusion positioning or the sensor fusion positioning method when executing the program.
Unless specifically stated otherwise, terms such as processing, computing, calculating, determining, displaying, or the like, may refer to an action and/or process of one or more processing or computing systems or similar devices that manipulates and transforms data represented as physical (e.g., electronic) quantities within the processing system's registers and memories into other data similarly represented as physical quantities within the processing system's memories, registers or other such information storage, transmission or display devices. Information and signals may be represented using any of a variety of different technologies and techniques. For example, data, instructions, commands, information, signals, bits, symbols, and chips that may be referenced throughout the above description may be represented by voltages, currents, electromagnetic waves, magnetic fields or particles, optical fields or particles, or any combination thereof.
It should be understood that the specific order or hierarchy of steps in the processes disclosed is an example of exemplary approaches. Based upon design preferences, it is understood that the specific order or hierarchy of steps in the processes may be rearranged without departing from the scope of the present disclosure. The accompanying method claims present elements of the various steps in a sample order, and are not intended to be limited to the specific order or hierarchy presented.
In the foregoing detailed description, various features are grouped together in a single embodiment for the purpose of streamlining the disclosure. This method of disclosure is not to be interpreted as reflecting an intention that the claimed embodiments of the subject matter require more features than are expressly recited in each claim. Rather, as the following claims reflect, the disclosure may lie in less than all features of a single disclosed embodiment. Thus, the following claims are hereby expressly incorporated into the detailed description, with each claim standing on its own as a separate preferred embodiment of the disclosure.
Those of skill would further appreciate that the various illustrative logical blocks, modules, circuits, and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware, computer software, or combinations of both. To clearly illustrate this interchangeability of hardware and software, various illustrative components, blocks, modules, circuits, and steps have been described above generally in terms of their functionality. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the overall system. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present disclosure.
The steps of a method or algorithm described in connection with the embodiments disclosed herein may be embodied directly in hardware, in a software module executed by a processor, or in a combination of the two. A software module may reside in RAM memory, flash memory, ROM memory, EPROM memory, EEPROM memory, registers, hard disk, a removable disk, a CD-ROM, or any other form of storage medium known in the art. An exemplary storage medium is coupled to the processor such the processor can read information from, and write information to, the storage medium. Of course, the storage medium may also be integral to the processor. The processor and the storage medium may reside in an ASIC. The ASIC may reside in a user terminal. Of course, the processor and the storage medium may reside as discrete components in a user terminal.
For a software implementation, the techniques described herein may be implemented with modules (e.g., procedures, functions, and so on) that perform the functions described herein. The software codes may be stored in memory units and executed by processors. The memory unit may be implemented within the processor or external to the processor, in which case it can be communicatively coupled to the processor via various means as is known in the art.
What has been described above includes examples of one or more embodiments. It is, of course, not possible to describe every conceivable combination of components or methodologies for purposes of describing the aforementioned embodiments, but one of ordinary skill in the art may recognize that many further combinations and permutations of various embodiments are possible. Accordingly, the embodiments described herein are intended to embrace all such alterations, modifications and variations that fall within the scope of the appended claims. Furthermore, to the extent that the term "includes" is used in either the detailed description or the claims, such term is intended to be inclusive in a manner similar to the term "comprising" as "comprising" is interpreted when employed as a transitional word in a claim. Furthermore, any use of the term "or" in the specification of the claims is intended to mean a "non-exclusive or". The terms "first," "second," and "third" are used for descriptive purposes only and are not to be construed as indicating or implying relative importance.
Claims (10)
1. A covariance determination method for sensor fusion localization, comprising:
a parameter arranging step, comprising: acquiring a sensor parameter with a change step length larger than a set step length threshold value obtained by current iteration in a positioning process as a first parameter;
the Jacobian matrix resolving step comprises the following steps: updating a current Jacobian matrix according to a residual factor obtained by current iteration and a first parameter obtained in the parameter arrangement step, wherein the residual factor is determined according to measurement data of a sensor and sensor parameters;
the Hessian matrix solving step comprises the following steps: updating the current Hessian matrix according to the split dense sub-matrix in the updated Jacobian matrix, returning to the parameter arrangement step until an iteration end condition is met, and outputting a final Hessian matrix;
a decomposition step comprising: decomposing the Hessian matrix to obtain an upper triangular matrix;
a covariance solution step including: and determining a covariance matrix representing the accuracy and the correlation of the first parameter in a recursive mode according to the upper triangular matrix.
2. The method according to claim 1, wherein updating the current jacobian matrix according to the residual factor obtained from the current iteration and the first parameter obtained from the parameter arrangement step specifically includes:
aiming at each residual error factor obtained by current iteration, determining an updated value of a corresponding element of the residual error factor in a Jacobian matrix according to a first parameter contained in the residual error factor;
determining the position of the corresponding element of the residual factor in the Jacobian matrix according to the identifier of the residual factor and the identifier of the first parameter contained in the residual factor;
and replacing the values of the elements of the position in the current Jacobian matrix with the corresponding updated values.
3. The method according to claim 1, wherein the updating the current hessian matrix according to the split dense sub-matrices in the updated jacobian matrix specifically comprises:
for each row in the updated Jacobian matrix, starting from a first non-zero element, splitting at least two continuous non-zero elements into a dense sub-matrix, and determining the value of an updated sub-element according to the dense sub-matrix;
determining the position of the corresponding element of the updated sub-element in the Hessian matrix according to the identifier of the first parameter corresponding to the updated sub-element;
and replacing the value of the element of the position in the current Hessian matrix with the sum of the values of the corresponding updating sub-elements.
4. The method according to claim 1, wherein determining the covariance matrix characterizing the accuracy and correlation of the first parameter recursively according to the upper triangular matrix comprises:
determining the value of an element at a corresponding position in a covariance matrix according to the value of a diagonal element with the maximum row number and the maximum column number in the upper triangular matrix, wherein the total row number and the total column number of the element in the covariance matrix are respectively consistent with the total row number and the total column number of the element in the upper triangular matrix;
and determining the values of the elements with undetermined values in the covariance matrix according to the sequence of the row and column numbers from large to small, the diagonal elements first and the off-diagonal elements sequentially in a recursion mode according to the values of the corresponding elements in the upper triangular matrix and the values of the corresponding elements in the covariance matrix.
5. The method according to claim 4, wherein the determining, in a recursive manner, values of elements having values that are not determined in the covariance matrix according to values of corresponding elements in the upper triangular matrix and values of corresponding elements in the covariance matrix in turn comprises:
when the element of undetermined dereferencing in the covariance matrix is a diagonal element, determining a first row number and a first column number of the element, and determining the value of the element of undetermined dereferencing according to the value of the element of which the row number is the same as the first row number in the upper triangular matrix and the value of the element of which the row number is greater than the first row number and the column number is the same as the first column number in the covariance matrix;
and when the element with the undetermined dereferencing in the covariance matrix is a diagonal non-angular line element, determining a second row number and a second column number of the element, and according to the values of the elements with the row numbers same as the second row number in the upper triangular matrix, determining the value of the element with the undetermined dereferencing according to the values of the elements with the row numbers larger than the second row number and the column numbers same as the second column number in the covariance matrix and the values of the elements with the column numbers same as the second column number and the row numbers larger than the second row number in the covariance matrix.
6. The method of any of claims 1 to 5, further comprising:
determining target parameters from sensor parameters needing to be optimized according to a set rule; accordingly, the method can be used for solving the problems that,
the acquiring of the sensor parameter with the change step length larger than the set step length threshold obtained by the current iteration in the positioning process is a first parameter, and specifically includes:
and acquiring a target parameter with a change step length larger than a set step length threshold value obtained by current iteration in the positioning process from the target parameter as a first parameter.
7. The method according to any one of claims 1 to 5, further comprising, after performing the covariance solution step:
determining the accuracy of the corresponding sensor parameter according to the diagonal elements in the covariance matrix; and/or the presence of a gas in the gas,
and determining the correlation of the parameters of every two sensors according to the elements in the covariance matrix.
8. A sensor fusion positioning method comprises the following steps:
determining the accuracy of the corresponding sensor parameter from diagonal elements in the covariance matrix determined according to the method of any of claims 1-7;
and taking the parameter with the accuracy greater than the set threshold value as an initial value of the corresponding parameter in the next sensor fusion positioning.
9. A covariance determination apparatus for sensor fusion localization, comprising:
the parameter arranging module is used for acquiring a sensor parameter of which the change step length obtained by current iteration in the positioning process is larger than a set step length threshold value as a first parameter;
the Jacobian matrix resolving module is used for updating a current Jacobian matrix according to a residual factor obtained by current iteration and a first parameter obtained by the parameter arranging module, wherein the residual factor is determined according to measurement data of a plurality of sensors and sensor parameters;
the hessian matrix solving module is used for updating the current hessian matrix according to the split dense sub-matrix in the updated Jacobian matrix obtained by the Jacobian matrix solving module; the parameter arranging module, the Jacobian matrix solving module and the Hessian matrix solving module work circularly until iteration is finished, and the Hessian matrix solving module is also used for outputting a final Hessian matrix;
the decomposition module is used for decomposing the Hessian matrix output by the Hessian matrix solving module to obtain an upper triangular matrix;
and the covariance calculation module is used for determining a covariance matrix representing the accuracy and the correlation of the first parameter in a recursive mode according to the upper triangular matrix obtained by the decomposition module.
10. A computer program product comprising a computer program/instructions, wherein the computer program/instructions, when executed by a processor, implement the covariance determination method for sensor fusion localization of any one of claims 1-7, or implement the sensor fusion localization method of claim 8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110042412.1A CN112765548B (en) | 2021-01-13 | 2021-01-13 | Covariance determination method, positioning method and device for sensor fusion positioning |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110042412.1A CN112765548B (en) | 2021-01-13 | 2021-01-13 | Covariance determination method, positioning method and device for sensor fusion positioning |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112765548A true CN112765548A (en) | 2021-05-07 |
| CN112765548B CN112765548B (en) | 2024-01-09 |
Family
ID=75700098
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110042412.1A Active CN112765548B (en) | 2021-01-13 | 2021-01-13 | Covariance determination method, positioning method and device for sensor fusion positioning |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112765548B (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114881124A (en) * | 2022-04-21 | 2022-08-09 | 北京百度网讯科技有限公司 | Method and device for constructing cause-and-effect relationship diagram, electronic equipment and medium |
| US20230008651A1 (en) * | 2021-07-07 | 2023-01-12 | Rohde & Schwarz Gmbh & Co. Kg | Jitter determination method and measurement instrument |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109993113A (en) * | 2019-03-29 | 2019-07-09 | 东北大学 | A kind of position and orientation estimation method based on the fusion of RGB-D and IMU information |
| WO2019136613A1 (en) * | 2018-01-09 | 2019-07-18 | 深圳市沃特沃德股份有限公司 | Indoor locating method and device for robot |
| WO2020107930A1 (en) * | 2018-11-29 | 2020-06-04 | 南京人工智能高等研究院有限公司 | Camera pose determination method and apparatus, and electronic device |
| CN111678515A (en) * | 2020-04-28 | 2020-09-18 | 北京三快在线科技有限公司 | Device state estimation method and device, electronic device and storage medium |
-
2021
- 2021-01-13 CN CN202110042412.1A patent/CN112765548B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2019136613A1 (en) * | 2018-01-09 | 2019-07-18 | 深圳市沃特沃德股份有限公司 | Indoor locating method and device for robot |
| WO2020107930A1 (en) * | 2018-11-29 | 2020-06-04 | 南京人工智能高等研究院有限公司 | Camera pose determination method and apparatus, and electronic device |
| CN109993113A (en) * | 2019-03-29 | 2019-07-09 | 东北大学 | A kind of position and orientation estimation method based on the fusion of RGB-D and IMU information |
| CN111678515A (en) * | 2020-04-28 | 2020-09-18 | 北京三快在线科技有限公司 | Device state estimation method and device, electronic device and storage medium |
Non-Patent Citations (2)
| Title |
|---|
| 余娟, 颜伟, 徐国禹, 杜鹏, 刘方: "基于预测-校正原对偶内点法的无功优化新模型", 中国电机工程学报, no. 11 * |
| 季晨;宋燕燕;秦军;敖甜甜;: "非线性优化同时定位与地图创建问题", 软件工程, no. 09 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20230008651A1 (en) * | 2021-07-07 | 2023-01-12 | Rohde & Schwarz Gmbh & Co. Kg | Jitter determination method and measurement instrument |
| CN114881124A (en) * | 2022-04-21 | 2022-08-09 | 北京百度网讯科技有限公司 | Method and device for constructing cause-and-effect relationship diagram, electronic equipment and medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112765548B (en) | 2024-01-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109373999B (en) | Integrated navigation method based on fault-tolerant Kalman filtering | |
| CN112765548A (en) | Covariance determination method, positioning method and device for sensor fusion positioning | |
| US11796686B2 (en) | Positioning data processing method and processing apparatus, computing device, and storage medium | |
| CN111461106A (en) | Object detection method and device based on reconfigurable network | |
| WO2012071320A1 (en) | Coded filter | |
| CN107015248B (en) | Efficient covariance matrix update | |
| CN114136315A (en) | Monocular vision-based auxiliary inertial integrated navigation method and system | |
| CN114689047A (en) | Deep learning-based integrated navigation method, device, system and storage medium | |
| CN114442133A (en) | Unmanned aerial vehicle positioning method, device, equipment and storage medium | |
| CN110912535B (en) | Novel non-pilot Kalman filtering method | |
| CN111623764B (en) | Micro-nano satellite attitude estimation method | |
| CN113252023A (en) | Positioning method, device and equipment based on odometer | |
| CN114485651B (en) | FLS and UKF combined navigation method, device, storage medium and equipment | |
| CN107132521B (en) | method for judging correctness of terrain matching result in BSLAM (binary-coded decimal motion) | |
| CN114001976B (en) | Method, device, equipment and storage medium for determining control error | |
| Ovchinnikov et al. | Windowed multiscan optimization using weighted least squares for improving localization accuracy of mobile robots | |
| CN115388878A (en) | Map construction method and device and terminal equipment | |
| CN113624219A (en) | Magnetic compass ellipse fitting error compensation method based on OPTICS algorithm | |
| CN116050554A (en) | Scenic spot passenger flow volume prediction method and device, computing equipment and storage medium | |
| CN114323007A (en) | Carrier motion state estimation method and device | |
| CN117523119A (en) | Ground map point cloud increment construction method and device based on remote sensing image guidance | |
| CN113899357B (en) | Incremental mapping method and device for visual SLAM, robot and readable storage medium | |
| CN117824667B (en) | Fusion positioning method and medium based on two-dimensional code and laser | |
| JP2019104432A (en) | Correction device, system, correction method, and program | |
| CN114926549B (en) | Three-dimensional point cloud processing method, device, equipment and storage medium |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |