Uniform motion rigid body positioning method based on arrival time

Abstract

The invention discloses a uniform motion rigid body positioning method based on arrival time. The uniform motion rigid body positioning method comprises the following steps of 1, describing global position coordinates of a sensor during k-th measurement in a 1-dimensional sensor network through the following formula; 2, according to # imgabs0 #, establishing a distance measurement model between the ith sensor and the mth anchor point during the k-th measurement; 3, carrying out a conversion process on the distance measurement model; step 4, constructing a weighted least square problem with constraint conditions based on an equation # imgabs1 #; step 5, relaxing a weighted least square problem into a convex semi-definite programming problem for solving; step 6, performing orthogonalization on the first rotation matrix # imgabs2 # when the first rotation matrix # imgabs2 # is # imgabs3 # until det (Xn + 1)-1lt; 10-15, a second rotation matrix # imgabs4 # is obtained; 7, a position vector # imgabs6 #, an angular velocity vector # imgabs7 # and a translation velocity vector # imgabs8 # of the second rotation matrix # imgabs5 # are corrected; according to the method, accurate estimation of the position and motion parameters of the rigid body can be realized in an environment with limited resources or large noise.

Description

Technical Field

[0001] The invention relates to a target positioning technology, in particular to a uniform motion rigid body positioning method based on arrival time. Background Art

[0002] Traditional target positioning methods regard the target as a point source and determine its position by receiving signals from sensors. They are widely used in fields such as communication, tracking, and monitoring. However, targets in many real-world scenarios have shapes and volumes. For example, in applications such as autonomous driving and robotics, it is necessary not only to obtain the position but also the direction and motion state. Inertial measurement units (IMUs) and global positioning systems (GPS) are commonly used methods, but IMUs are susceptible to drift and GPS is prone to failure in shielded environments, limiting positioning accuracy.

[0003] The rigid body positioning method models the target as a rigid body to more accurately describe its spatial distribution and dynamic characteristics. Usually, multiple sensors with known positions are installed on the surface of the rigid body, and the rotation matrix and position vector are determined by measuring the arrival time (TOA) or arrival angle (AOA). However, the nonlinear coupling relationship between the rotation matrix and the position vector, and the need for the rotation matrix to satisfy specific orthogonal constraints, make the optimization of this problem more complicated. When the rigid body is in motion, the translational velocity and angular velocity must be additionally obtained, making positioning more challenging than traditional point source positioning.

[0004] Existing rigid body positioning methods usually combine TOA and Doppler measurements to obtain the position information of the rigid body. However, in some practical application scenarios, especially when resources are limited or the environment is complex, it is difficult to accurately obtain Doppler information. Therefore, TOA-based measurement becomes a more practical option. Summary of the invention

[0005] The technical problem to be solved by the present invention is to provide a method for positioning a uniformly moving rigid body based on arrival time, which can achieve accurate estimation of the position and motion parameters of the rigid body in an environment with limited resources or high noise, and avoids the reliance of traditional methods on Doppler measurement.

[0006] The technical solution adopted by the present invention to solve the above technical problems is:

[0007] A method for positioning a rigid body in uniform motion based on arrival time comprises the following steps:

[0008] Step 1: The global position coordinates of the sensor at the kth measurement in the l-dimensional sensor network are described by the following formula, namely:

[0009]

[0010] Among them; N sensors are installed on the rigid body, and the local coordinates of the i-th sensor are given by c i , i=1,…,N represents; outside the rigid body, there are M anchor nodes with known positions, and their global coordinates are represented by a m ,m=1,2,…,M; K is the total number of measurements, T is the time interval between measurements;

[0011] is the rotation matrix with kTω as the Euler angle; the position vector of the rigid body is t o For a rigid body moving at a uniform speed, its overall motion is given by the angular velocity ω o and translation speed v o To represent; at the initial moment, the position of the i-th sensor in the global coordinate system is represented by Represented by the rotation matrix Q o and t o pass Related; where: the rotation matrix must satisfy the conditions of the special orthogonal group: Among them: the superscript "T" means the transpose of a matrix or vector, det() means finding the determinant of the matrix;

[0012] Step 2: According to Establish the distance measurement model between the i-th sensor and the m-th anchor point during the k-th measurement:

[0013]

[0014] in, Represents the true distance, subject to zero-mean Gaussian additive noise || || is the Euclidean norm operator, n im,k represents the measurement noise between the i-th sensor and the m-th anchor point at the k-th measurement; Represents the noise n im,k Power;

[0015] Step 3: Transform the distance measurement model. The specific transformation process is as follows:

[0016] Step 301: During the kth measurement, the rigid body rotation angle changes relative to the initial position Close to zero, use the small angle approximation cosx≈1, sinx≈x to get the updated rotation matrix for the kth measurement:

[0017]

[0018] Where: I is the identity matrix, s x = sinx o , c x =cosxo ; [ω o ] × The vector form of

[0019] vec([ω o ] × )=Φω o ; Among them: vec() means vectorizing the matrix,

[0020]

[0021] Step 302: According to The global coordinates of sensor i at the kth measurement are transformed into:

[0022]

[0023] Among them: intermediate variable P o =[ω o ] × Q o By estimating P o , indirectly derive [ω o ] × The value of

[0024] Step 303: Substitute n into the distance measurement model and replace im,k Moving to the left, squaring both sides of the equation; neglecting the second-order noise terms yields:

[0025]

[0026] Step 304: Using vectorized formula The equation in step 303 is transformed into:

[0027]

[0028] in: represents the Kronecker product operation;

[0029] Step 305: Simplify the transformed measurement model into in:

[0030]

[0031]

[0032] A k Every behavior

[0033]

[0034] Step 4: Based on the equation Construct a weighted least squares problem with constraints, described as:

[0035]

[0036] in: It means that the goal is to minimize the objective function (Ay-b) under given constraints. T W(Ay-B), st means "under the following conditions", y is the value corresponding to the unknown quantity y o The optimization variable, weight matrix is ​​W = (BRB T ) -1 ,in() -1 Indicates the inversion of the matrix;

[0037] Step 5: Relax the weighted least squares problem into a convex semidefinite programming problem and solve it:

[0038] Step 501: Introduce auxiliary matrix Y=yy T , according to the equivalence of Y, we get the constraint condition Ignoring the constraints det(Q) = 1 and rank(Y) = 1, the weighted least squares problem with constraints is relaxed into a convex semi-definite programming problem using semi-definite relaxation techniques, which can be described as:

[0039]

[0040] Step 502: Solve the above convex semi-positive definite programming problem through the following steps:

[0041] Set the weight matrix W to the identity matrix and solve the optimization problem to obtain the initial estimate;

[0042] Use the initial estimate to update the weight matrix and solve it again to get the final result of the optimization problem;

[0043] In finding the solution to the optimization problem Then we get the rotation matrix Q o The initial estimate of the first rotation matrix Position vector t o Initial estimate of Translational velocity vector v o Initial estimate of Intermediate variable P o Initial estimate of

[0044]

[0045] Among them: the matrix mat{a,k,l} is a k×l matrix obtained by splitting the vector a into l sub-vectors and rearranging them. is the vector consisting of the i-th to j-th elements in the intercepted vector;

[0046] Step 6: For the first rotation matrix exist When, orthogonalization is performed until |det(X n+1 )-1|<10 -15 Get the second rotation matrix

[0047]

[0048] Among them: X0 is the initial matrix of the orthogonalization process, X n is the matrix at the nth iteration, X n+1 is the updated matrix at the n+1th iteration, Represents the matrix X n Take the inverse transpose, |a| means taking the absolute value of element a; after obtaining the orthogonalized solution, the second rotation matrix Afterwards, through and Construct [ω o ] × and the angular velocity vector ω o Estimates:

[0049] Step 7: For the second rotation matrix Position vector Angular velocity vector and translation velocity vector Correction process:

[0050] Step 701: construct a linear equation according to the correction amount:

[0051] Let △t o , △ω o , △v o To correct the estimated value, the true value is expressed as: Q o Decompose into

[0052] in, is the Euler angle vector The modified rotation matrix is ​​formed; after small angle approximation

[0053] where γ=[1,0,0,0,1,0,0,0,1] T ,

[0054] Will Substitute into the equation in step 303, ignoring the second-order and third-order correction terms; where, when When multiplied by other correction terms, Approximately the identity matrix; then use the vectorized formula and get

[0055] in: H=[H1,H2,H3,H4],

[0056] H1 Behavior

[0057] Behavior of H2

[0058] H3 Behavior

[0059] H4 Behavior Behavior

[0060] Step 702: According to the linear equation Construct an optimization problem about g: where g is g o The corresponding optimization variables;

[0061] Its linear weighted least squares solution is

[0062] from get After that, use structure Estimates Reuse Deriving the final estimate ξ o Final estimate Depend on calculate: arctan2(p,q) is a four-quadrant inverse tangent operation;

[0063] Will and Add to initial solution and On the top, we get the corrected solution:

[0064]

[0065] Beneficial Effects

[0066] Compared with the prior art, the advantages of the present invention are:

[0067] 1) Compared with the complex system in the existing method that needs to rely on both arrival time and Doppler information for positioning, the present invention only relies on arrival time measurement. Traditional methods need to deal with the nonlinear coupling relationship between position and velocity at the same time. The present invention avoids the complex fusion of multiple information by using only arrival time information to locate uniformly moving rigid bodies.

[0068] 2) The present invention transforms the original nonlinear measurement model into a convex semidefinite programming problem that is easier to solve, greatly simplifying the difficulty of problem solving.

[0069] 3) The solution proposed in this invention demonstrates excellent noise robustness and can generate solutions close to the Cramer-Rao lower bound (CRLB) even in high noise environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0070] Figure 1 It is a general implementation block diagram of the method of the present invention;

[0071] Figure 2 Positioning scene graph for 3D rigid bodies;

[0072] Figure 3 When M=5, N=4, K=5, and T=0.1s, the mean square error (MSE) and Cramer-Rao lower bound (CRLB) of the estimated values ​​of the rotation angle ξ and the angular velocity vector ω according to the method of the present invention vary with the average power σ of the TOA measurement noise. 2 Comparative graph of changes;

[0073] Figure 4 When M=5, N=4, K=5, and T=0.1s, the mean square error (MSE) and Cramer-Rao lower bound (CRLB) of the estimated values ​​of the position vector t and the translation velocity vector v according to the method of the present invention vary with the average power σ of the TOA measurement noise. 2 Comparative graph of changes;

[0074] Figure 5 N = 4, K = 5, T = 0.1s, σ 2 =10 -1 m 2 Below, a comparison diagram of the mean square error (MSE) and Cramer-Rao lower bound (CRLB) of the estimated values ​​of the rotation angle ξ and the angular velocity vector ω according to the method of the present invention as the number of anchor points changes;

[0075] Figure 6 N = 4, K = 5, T = 0.1s, σ 2 =10 -1 m 2 Below, a comparison diagram of the mean square error (MSE) and Cramer-Rao lower bound (CRLB) of the estimated values ​​of the position vector t and the translation velocity vector v according to the method of the present invention as the number of anchor points changes;

[0076] Figure 7 M = 5, K = 5, T = 0.1s, σ 2 =10 -1 m 2 Below, a comparison diagram of the mean square error (MSE) and Cramer-Rao lower bound (CRLB) of the estimated values ​​of the rotation angle ξ and the angular velocity vector ω according to the method of the present invention as the number of sensors changes;

[0077] Figure 8 M = 5, K = 5, T = 0.1s, σ 2 =10 -1 m 2 Below is a comparison diagram of the mean square error (MSE) and Cramer-Rao lower bound (CRLB) of the estimated values ​​of the position vector t and the translation velocity vector v according to the method of the present invention as the number of sensors changes. DETAILED DESCRIPTION

[0078] The present invention is further described in detail below with reference to the accompanying drawings.

[0079] The present invention proposes a uniform motion rigid body positioning method based on multiple arrival time (TOA) measurements, and its overall implementation block diagram is as follows: Figure 1 As shown, it includes the following steps:

[0080] Step 1: Construct the global position coordinates of the sensor at the kth measurement in the l-dimensional sensor network

[0081] In the l sensor network, a global reference coordinate system is established. Assume that there is a rigid body with unknown position. Inside the rigid body, a rigid body local coordinate system is established, where N sensors are installed on the rigid body. The local coordinates of the i-th sensor are represented by c i , i=1,…,N represents. Outside the rigid body, there are M anchor nodes with known positions, and their global coordinates are represented by a m ,m=1,2,…,M, Figure 2 A positioning scene graph in three-dimensional space is given.

[0082] By c i The sensor topology is composed of a rotation matrix Q, whose orientation is represented by the rotation matrix Q, and its position relative to the global coordinate reference system is represented by the position vector t o Represents. The rotation matrix must satisfy the conditions of the special orthogonal group The superscript "T" indicates the transposition of a matrix or vector, and det() indicates the determination of the matrix determinant. The position of the i-th sensor in the global coordinate system is expressed as Indicates that Q o and t o pass For a rigid body moving at a uniform speed, its overall motion is determined by the angular velocity ωo and translation speed v o To characterize. A distance measurement is performed every time interval T. For the kth measurement, the position of the i-th sensor mounted on the rigid body in the global coordinate system is updated as in is the rotation matrix with kTω as the Euler angle; K is the total number of measurements, and T is the measurement time interval.

[0083] Step 2: Establish a distance measurement model based on multiple measurements of arrival time at equal time intervals, that is: Establish the distance measurement model between the i-th sensor and the m-th anchor point during the k-th measurement

[0084]

[0085] in, Represents the true distance, subject to zero-mean Gaussian additive noise The influence of || || is the Euclidean norm operator. The measurement noises of different sensors are independent of each other. Consider the kth measurement, its noise vector n k =[n 11,k ,…,n 1M,k ,…,n N1,k ,…,n NM,k ] T The covariance matrix of Further consider all the measurement noise vectors Its covariance matrix is ​​R = Diag{R1,…,R K The measurement model is expressed in vector form as d = d o +n.Among them Indicates the creation of a is a diagonal matrix with diagonal elements; Diag{R1,…,R K} represents the matrix R1,…,R K Combine diagonal blocks into a larger diagonal matrix; n im,k is the measurement noise between the ith sensor and the mth anchor point at the kth measurement; is the measurement noise power between the ith sensor and the mth anchor point at the kth measurement; n k is the column vector formed by the measurement noise between N sensors and M anchor points during the kth measurement; R k n k The covariance matrix of .

[0086] Step 3: Construct an updated rotation matrix to transform the measurement model. The specific transformation process is as follows:

[0087] In three-dimensional space, the angular velocity of a rigid body is in Represents the rotation speed of the rigid body around the X, Y, and Z axes respectively. The rigid body is measured K times, and the measurement time interval is T. At the kth measurement, the rigid body rotation angle changes relative to the initial position Close to zero, use the small angle approximation cosx≈1, sinx≈x to get the updated rotation matrix for the kth measurement

[0088]

[0089] Where: I is the identity matrix, s x = sinx o , c x =cosx o , [ω o ] × The vector form of vec() means vectorizing the matrix.

[0090] use The global coordinates of sensor i at the kth measurement are expressed as

[0091]

[0092] Among them: the intermediate variable P is defined o =[ω o ] × Q o By estimating P o , indirectly derive [ω o ] × The value of

[0093] Will Substitute into the distance measurement model constructed in step 2, and replace n im,k Moving to the left, squaring both sides of the equation; neglecting the second-order noise terms yields:

[0094] Using vectorized formulas represents the Kronecker product operation, which can be further written as

[0095] Where: Ignore the second-order noise term.

[0096] The transformed measurement model is simplified to in

[0097]

[0098] A k Every behavior where d im,krepresents the distance between the i-th sensor and the m-th anchor point at the k-th measurement; d k is a vector containing the distances between all sensors and the anchor point at the kth measurement.

[0099] Step 4: Based on the equation Construct a weighted least squares problem with constraints, described as:

[0100]

[0101] in It means that the goal is to minimize the objective function (Ay-b) under given constraints. T W(Ay-B), st means "under the following conditions", y is the value corresponding to the unknown quantity y o The optimization variable, weight matrix is ​​W = (BRB T ) -1 ,in() -1 Indicates the inversion of the matrix;

[0102] Step 5: Relax the weighted least squares problem into a convex semidefinite programming problem.

[0103] Step 501: Introduce auxiliary matrix Y=yy T , according to the equivalence of Y, we get the constraint condition Ignoring the constraints det(Q) = 1 and rank(Y) = 1, the weighted least squares problem with constraints is relaxed into a convex semi-definite programming problem using semi-definite relaxation techniques, which can be described as:

[0104]

[0105] Step 502: Solve the above convex semi-positive definite programming problem through the following steps:

[0106] Set the weight matrix W to the identity matrix and solve the optimization problem to obtain the initial estimate;

[0107] Use the initial estimate to update the weight matrix and solve again to get the final result of the optimization problem. Then we get the initial estimates of the parameters:

[0108] Among them: the matrix mat{a,k,l} is a k×l matrix obtained by splitting the vector a into l sub-vectors and rearranging them. is the vector consisting of the i-th to j-th elements in the intercepted vector; the first rotation matrix is the rotation matrix Q o The initial estimate of is the position vector t oThe initial estimate of is the translational velocity vector v o The initial estimate of is the intermediate variable P o initial estimate of .

[0109] Step 6: For the first rotation matrix exist When, orthogonalization is performed until |det(X n+1 )-1|<10 -15 Get the second rotation matrix

[0110] The orthogonalization step ensures that When , the first rotation matrix estimated in step 5 satisfies the condition of SO(l): Until |det(X n+1 )-1|<10 -15 .

[0111] Among them: X0 is the initial matrix of the orthogonalization process, X n is the matrix at the nth iteration, X n+1 is the updated matrix at the n+1th iteration, Represents the matrix X n Take the inverse transpose, |a| means taking the absolute value of element a. After orthogonalization, the second rotation matrix is ​​obtained. Afterwards, through and Construct [ω o ] × and angular velocity ω o Estimates:

[0112] Step 7: For the second rotation matrix Position vector Angular velocity vector and translation velocity vector Correction process:

[0113] Step 701: construct a linear equation according to the correction amount:

[0114] Let △t o , △ω o , △v o To correct the estimated value, the true value is expressed as:

[0115] Will Breaking down into multiplication form: in, is the Euler angle vector The modified rotation matrix is ​​formed; after small angle approximation

[0116] Where: γ=[1,0,0,0,1,0,0,0,1] T ,

[0117] Will Substitute into the step equation Ignore the second-order and third-order correction terms; when When multiplied by other correction terms, Approximately the identity matrix; then use the vectorized formula and get

[0118] Where: g o =[△ξ o ,△t oT ,△ω o ,△v oT ] T , H=[H1,H2,H3,H4],

[0119] H1 Behavior

[0120] Behavior of H2

[0121] H3 Behavior

[0122] H4 Behavior Behavior

[0123]

[0124] Step 702: According to the linear equation Construct an optimization problem about g: Construct an optimization problem where g is g o The corresponding vector variable. Its linear weighted least squares (LWLS) solution is

[0125] In from get After that, use structure Estimates Reuse Deriving the final estimate ξ o Final estimate Depend on calculate: in Respectively The horizontal coordinate, vertical coordinate and vertical coordinate of arctan2(p,q) is a four-quadrant inverse tangent operation. and Add to initial solution and On the top, we get the corrected solution:

[0126] In order to verify the feasibility and effectiveness of the method of the present invention, a simulation experiment was carried out on the method of the present invention.

[0127] Assume that in three-dimensional space, five anchor points are randomly distributed in a cube with a side length of 60 meters, and the center of the cube is located at the origin of the global coordinate system. To avoid geometric degradation, the minimum distance between any two anchor points is 10 meters. The rigid body is located in a cube with a side length of 90 meters. Four sensors are installed on the rigid body, and their local coordinates are the first 4 columns of C.

[0128]

[0129] The rotation angles of the rigid body around the x, y and z axes are π / 6rad, -π / 9rad, π / 4rad respectively. The angular velocity and translational velocity are ω o =[0.1,0.1,0.1] T rad / s and v o =[1,2,3] T m / s. The number of TOA measurements is K = 5, and the time interval is T = 0.1 s.

[0130] The performance of the proposed method was tested under different distance measurement noise variances. The number of anchor points was fixed to 5, the number of sensors was fixed to 4, 10 different scenes were randomly selected, and 1000 Monte Carlo experiments were performed on each scene. Figure 3 and Figure 4 The comparison between the estimated mean squared error (MSE) and the Cramer-Rao lower bound (CRLB) under different noise variances is shown. It can be seen that as the noise increases, our method can achieve CRLB even at higher noise levels. Figure 5 and Figure 6 Shows that in σ 2 =10 -1 m 2 Comparison of the mean square error (MSE) estimated by the method of the present invention and the Cramer-Rao lower bound (CRLB) with the number of anchor points. Figure 7 and Figure 8 Shows that in σ 2 =10 -1 m 2In the case of , the comparison between the mean square error (MSE) estimated by the method of the present invention and the Cramer-Rao lower bound (CRLB) with the number of sensors varies, and the local coordinates of the sensors are the columns in C. By Figures 6 to 8 It can be seen that in σ 2 =10 -1 m 2 Under the noise level, the method of the present invention has a small requirement on the number of anchor points and sensors, and only 4 anchor points and 4 sensors are needed to achieve the Cramer-Rao lower bound (CRLB) accuracy.

Claims

1. A method for positioning a rigid body in uniform motion based on arrival time, characterized in that: The following steps are involved: Step 1: The global position coordinates of the sensor at the kth measurement in the l-dimensional sensor network are described by the following formula: in; N sensors are mounted on a rigid body, and the local coordinates of the i-th sensor are given by c i , i=1,…,N represents; outside the rigid body, there are M anchor nodes with known positions, and their global coordinates are represented by a m ,m=1,2,…,M; K is the total number of measurements, T is the time interval between measurements; is the rotation matrix with kTω as the Euler angle; the position vector of the rigid body is t o For a rigid body moving at a uniform speed, its overall motion is given by the angular velocity ω o and translation speed v o To represent; at the initial moment, the position of the i-th sensor in the global coordinate system is represented by Represented by the rotation matrix Q o and t o pass Related; where: the rotation matrix must satisfy the conditions of the special orthogonal group: Among them: the superscript "T" means the transposition of a matrix or vector, det() means finding the determinant of the matrix; Step 2: According to Establish the distance measurement model between the i-th sensor and the m-th anchor point during the k-th measurement: in, Represents the true distance, subject to zero-mean Gaussian additive noise The influence of || || is the Euclidean norm operator, n im,k represents the measurement noise between the i-th sensor and the m-th anchor point at the k-th measurement; Represents the noise n im,k Power; Step 3: Transform the distance measurement model. The specific transformation process is as follows: Step 301: During the kth measurement, the rigid body rotation angle changes relative to the initial position Close to zero, use the small angle approximation cosx≈1, sinx≈x to get the updated rotation matrix for the kth measurement: Where: I is the identity matrix, s x =sinx o , c x =cosx o ; [ω o ] × The vector form is vec([ω o ] × )=Φω o ; Among them: vec() means vectorizing the matrix, Step 302: According to The global coordinates of sensor i at the kth measurement are transformed into: Among them: intermediate variable P o =[ω o ] × Q o By estimating P o , indirectly derive [ω o ] × The value of Step 303: Substitute n into the distance measurement model and im,k Moving to the left, squaring both sides of the equation; neglecting the second-order noise terms yields: Step 304: Using vectorized formula The equation in step 303 is transformed into: in: represents the Kronecker product operation; Step 305: Simplify the transformed measurement model into in: A k Every behavior Step 4: Based on the equation Construct a weighted least squares problem with constraints, described as: in: The goal is to minimize the objective function under given constraints. (Ay-b) T W(Ay-B), st means "under the following conditions", y is the value corresponding to the unknown quantity y o The optimization variables, weight matrix is ​​W = (BRB T ) -1 ,in() -1 Indicates the inversion of the matrix; Step 5: Relax the weighted least squares problem into a convex semidefinite programming problem and solve it: Step 501: Introduce auxiliary matrix Y=yy T , according to the equivalence of Y, we get the constraint condition Ignoring the constraints det(Q) = 1 and rank(Y) = 1, the weighted least squares problem with constraints is relaxed into a convex semi-definite programming problem using semi-definite relaxation techniques, which can be described as: Step 502: Solve the above convex semi-positive definite programming problem through the following steps: Set the weight matrix W to the identity matrix and solve the optimization problem to obtain the initial estimate; Use the initial estimate to update the weight matrix and solve it again to get the final result of the optimization problem; In finding the solution to the optimization problem Then we get the rotation matrix Q o The initial estimate of the first rotation matrix Position vector t o Initial estimate of Translational velocity vector v o Initial estimate of Intermediate variable P o Initial estimate of Among them: the matrix mat{a,k,l} is a k×l matrix obtained by splitting the vector a into l sub-vectors and rearranging them. is the vector consisting of the i-th to j-th elements in the intercepted vector; Step 6: Rotate the first rotation matrix exist When, orthogonalization is performed until |det(X n+1 )-1|<10 -15 Get the second rotation matrix Among them: X0 is the initial matrix of the orthogonalization process, X n is the matrix at the nth iteration, X n+1 is the updated matrix at the n+1th iteration, Represents the matrix X n Take the inverse transpose, |a| means taking the absolute value of element a; after obtaining the orthogonalized solution, the second rotation matrix Afterwards, through and Construction [ω o ] × and the angular velocity vector ω o Estimates: Step 7: For the second rotation matrix Position vector Angular velocity vector and translation velocity vector Correction process: Step 701: construct a linear equation according to the correction amount: Let △t o , △ω o , △v o To correct the estimated value, the true value is expressed as: Q o Decompose into in, is the Euler angle vector The modified rotation matrix is ​​formed; after small angle approximation in Will Substitute into the equation in step 303, ignoring the second-order and third-order correction terms; where, when When multiplied by other correction terms, Approximately the identity matrix; then use the vectorized formula and get Where: g o =[△ξ o ,△t oT ,△ω o ,△v oT ] T , H=[H1,H2,H3,H4], H1 Behavior Behavior of H2 H3 Behavior H4 Behavior Behavior Step 702: According to the linear equation Construct an optimization problem about g: where g is g o The corresponding optimization variables; Its linear weighted least squares solution is from get After that, use structure Estimates Reuse Deriving the final estimate ξ o Final estimate Depend on calculate: arctan2(p,q) is a four-quadrant inverse tangent operation; Will and Add to initial solution and On the top, we get the corrected solution:

Images