St-ekf mean square error matrix initialization method and system based on error state correlation

Abstract

The application discloses an ST-EKF mean square error matrix initialization method and system based on error state correlation, and belongs to the field of integrated navigation control. Firstly, the traditional error state is used to calculate the mean square error matrix. Then, according to the transformation relationship between the nonlinear error state redefined by the ST-EKF and the traditional error state, the strong correlation between the new error state components is analyzed, and the error state conversion matrix is determined. Finally, the new error state is substituted into the mean square error matrix calculation formula, the corresponding mean square error matrix conversion model is obtained, and the initialization is completed. The method has good universality, and is suitable for various misalignment angles, especially in the case of large misalignment angle and high speed.

Description

Technical Field

[0001] This invention belongs to the field of integrated navigation control, and more specifically, relates to an ST-EKF mean square error matrix initialization method and system based on error state correlation. Background Technology

[0002] To address the linearization error problem in traditional integrated navigation system error state models, ST-EKF (State Transformed Extended Kalman Filtering) has attracted considerable attention from navigation researchers in recent years. ST-EKF considers the deviation between the calculated navigation coordinate system and the actual navigation coordinate system caused by attitude errors (i.e., misalignment angles), redefining the nonlinear velocity error state. Under large misalignment angles, it exhibits better estimation performance compared to traditional filtering algorithms. However, the initialization problem of the mean square error matrix corresponding to the transformed error state has not received sufficient attention.

[0003] The mean squared error matrix (MSEM) is defined as the expected value of the deviation between the state estimate and the true value. The diagonal elements of the matrix represent the variance of each state component estimate, and their square roots characterize the uncertainty of the state estimate. The off-diagonal elements of the MSEM are the covariances, reflecting the correlation between the estimation errors of different state components. Traditional inertial basis combination systems use the standard KF algorithm based on linear error states, which assumes that the initially estimated state errors are uncorrelated or weakly correlated. Therefore, the MSEM is usually initialized as a diagonal matrix. This method has also been applied to the initialization of the ST-EKF MSEM. Specifically, the traditional error states are first converted to new error states defined by ST-EKF, then the variance of the initial estimates of each new state component is calculated, and finally, the MSEM is set as a diagonal matrix.

[0004] However, ST-EKF redefines the nonlinear error state, and the components of each state exhibit strong correlation. It is unreasonable to roughly initialize the mean square error matrix with a diagonal matrix. Under conditions of large misalignment angles and high speeds, the estimation performance of the error state will be affected to some extent. Summary of the Invention

[0005] To address the shortcomings of existing technologies, the present invention aims to provide an ST-EKF mean square error matrix initialization method and system based on error state correlation, thereby solving the problem that the performance of error state estimation is affected by the prior art's initialization of the mean square error matrix as a diagonal matrix.

[0006] To achieve the above objectives, in a first aspect, the present invention provides an ST-EKF mean square error matrix initialization method based on error state correlation, comprising:

[0007] S1. Obtain the initial error state of the KF integrated navigation system, wherein the error state includes attitude error, velocity error and position error;

[0008] S2. Calculate the mean square error matrix of the KF integrated navigation system based on the initial error state;

[0009] S3. Determine the error state transformation matrix based on the transformation relationship between the nonlinear error state and the KF error state defined by ST-EKF;

[0010] S4. Based on the KF mean square error matrix and the error state transition matrix, obtain the mean square error matrix of the ST-EKF state-transformed integrated navigation system and complete the initialization.

[0011] Preferably, the mean square error matrix P of the KF integrated navigation system n The calculation formula is as follows:

[0012]

[0013] Among them, P φ P δv and P δp Let be the mean square error matrices corresponding to φ, δv, and δp, respectively, and all be diagonal matrices. Let φ, δv, and δp be the attitude error, velocity error, and position error, respectively. 3×3 It is a 3×3 zero matrix.

[0014] Preferably, the nonlinear error state defined according to ST-EKF is expressed as follows: δv φ The nonlinear velocity error δv after state transformation φ Represented as:

[0015]

[0016] Where δv is the attitude error. To estimate the velocity, (·×) is the antisymmetric matrix operator, and φ is the attitude error.

[0017] Preferably, the error state transition matrix A sn The calculation formula is as follows:

[0018]

[0019] Among them, I 3×3 It is a 3×3 identity matrix, 0 3×3 It is a 3×3 zero matrix. This is the antisymmetric matrix for estimating velocity.

[0020] Preferably, the mean square error matrix P of the integrated navigation system after the ST-EKF state transformation is... s The calculation formula is as follows:

[0021] P s =A sn P n A sn T

[0022] Among them, A sn Let P be the error state transition matrix. n Let be the mean square error matrix of the integrated navigation system.

[0023] To achieve the above objectives, in a second aspect, the present invention provides an ST-EKF mean square error matrix initialization system based on error state correlation, comprising: a processor and a memory; the memory being used to store computer execution instructions; and the processor being used to execute the computer execution instructions, such that the method described in the first aspect is executed.

[0024] To achieve the above objectives, in a third aspect, the present invention provides a computer-readable storage medium storing a computer program that, when executed on a processor, causes the processor to perform the method described in the first aspect.

[0025] In summary, the technical solutions conceived by this invention have the following beneficial effects compared with the prior art:

[0026] This invention provides a method and system for initializing the ST-EKF mean square error matrix based on the correlation of error states. First, the mean square error matrix is ​​calculated using traditional error states. Then, based on the transformation relationship between the newly defined nonlinear error states and the traditional error states in ST-EKF, the strong correlation between the components of the new error states is analyzed to determine the error state transformation matrix. Finally, the new error states are substituted into the mean square error matrix calculation formula to obtain the corresponding mean square error matrix transformation model, completing the initialization. This method has good versatility and is applicable to various misalignment angles, especially in situations with large misalignment angles and high speeds. Attached Figure Description

[0027] Figure 1 This is a flowchart of an ST-EKF mean square error matrix initialization method based on error state correlation provided by the present invention. Detailed Implementation

[0028] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0029] like Figure 1 As shown, this invention provides an ST-EKF mean square error matrix initialization method based on error state correlation, comprising:

[0030] S1. Obtain the initial error state of the KF integrated navigation system, wherein the error state includes attitude error, velocity error and position error.

[0031] S2. Calculate the mean square error matrix of the KF integrated navigation system based on the initial error state.

[0032] S3. Determine the error state transformation matrix based on the transformation relationship between the nonlinear error state defined by ST-EKF and the KF error state.

[0033] S4. Based on the KF mean square error matrix and the error state transition matrix, obtain the mean square error matrix of the ST-EKF state-transformed integrated navigation system and complete the initialization.

[0034] I. Setting the KF Mean Square Error Matrix

[0035] Let X be the KF error state vector of a traditional integrated navigation system. n =[φ T δv T δp T ] T Where φ, δv, and δp represent the misalignment angle (attitude error), velocity error, and position error, respectively. The corresponding error state estimates are: The superscript ~ indicates the estimate of the corresponding variable, and the superscript T indicates the transpose operator.

[0036] Initial error state estimation When the vector is 0, the mean square error matrix of the system can be expressed as:

[0037]

[0038] Where E(·) denotes the mathematical expectation operator, X represents n The i-th component. P n The diagonal elements represent the variance of each state component estimate, and their square roots characterize the uncertainty of the state estimate. The off-diagonal elements represent the covariance, reflecting the correlation between the estimation errors of different state components. When initializing the mean square error matrix, it can be roughly assumed that the initial estimates of the error state components are uncorrelated, i.e., the covariance satisfies...

[0039]

[0040] Therefore, (1) can be expressed as:

[0041]

[0042] Here, `diag(·)` is the diagonal matrix operator. Let...

[0043]

[0044] Among them, P φ P δv and P δp These are the mean square error matrices corresponding to φ, δv, and δp, respectively, and all are diagonal matrices.

[0045] II. Transition of ST-EKF Error States

[0046] The error state vector of ST-EKF can be expressed as: δv φ The nonlinear velocity error after the state transformation can be expressed as:

[0047]

[0048] in, To estimate velocity, (·×) is the antisymmetric matrix operator.

[0049] As can be seen from (5), the new speed error state has an additional item. It is closely related to the misalignment angle φ, that is, there is a strong correlation between the error states after transformation. Therefore, the new mean square error matrix should not be roughly set as a diagonal matrix when it is initialized.

[0050] New error state X s And the traditional error state X n The following transformation relationship is satisfied:

[0051]

[0052] Among them, I 3×3 It is a 3×3 identity matrix. To estimate the antisymmetric matrix of the velocity. Matrix A sn Characterizes the error state X s and X n The transformation relationship between them can be referred to as the state transformation matrix.

[0053] III. Setting the ST-EKF Mean Square Error Matrix

[0054] The mean square error matrix of the system after the ST-EKF state transformation can be expressed as:

[0055]

[0056] It is evident from (7) that the state transformation matrix A sn It also characterizes the mean square error matrix P s With P n The conversion relationship between them.

[0057] Verification section

[0058] Substituting (4) and (6) into (7) yields:

[0059]

[0060] From (8), we can see that the new mean square error matrix P s It is clearly no longer a diagonal matrix, and the elements on the diagonal have also changed, verifying the analysis above.

[0061] Those skilled in the art will readily understand that the above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for initializing the ST-EKF mean square error matrix based on error state correlation, characterized in that, include: S1. Obtain the initial error state of the KF integrated navigation system, wherein the error state includes attitude error, velocity error and position error; S2. Calculate the mean square error matrix of the KF integrated navigation system based on the initial error state; S3. Determine the error state transformation matrix based on the transformation relationship between the nonlinear error state and the KF error state defined by ST-EKF; S4. Based on the KF mean square error matrix and the error state transition matrix, obtain the mean square error matrix of the ST-EKF state-transformed integrated navigation system and complete the initialization.

2. The method as described in claim 1, characterized in that, The mean square error matrix of the KF integrated navigation system The calculation formula is as follows: in, 、 and They are respectively with 、 and The corresponding mean square error matrices are all diagonal matrices. 、 and These are attitude error, velocity error, and position error, respectively. It is a 3×3 zero matrix.

3. The method as described in claim 1, characterized in that, The nonlinear error state defined according to ST-EKF is expressed as follows: , The nonlinear velocity error after the state transformation is expressed as: in, For speed error, To estimate the speed, For antisymmetric matrix operators, This represents the attitude error.

4. The method as described in claim 1, characterized in that, The error state transition matrix The calculation formula is as follows: in, It is a 3×3 identity matrix. It is a 3×3 zero matrix. This is the antisymmetric matrix for estimating velocity.

5. The method as described in claim 1, characterized in that, The mean square error matrix of the integrated navigation system after the ST-EKF state transformation The calculation formula is as follows: in, This is the error state transition matrix. Let be the mean square error matrix of the integrated navigation system.

6. An ST-EKF mean square error matrix initialization system based on error state correlation, characterized in that, include: Processor and memory; The memory is used to store computer-executed instructions; The processor is configured to execute the computer execution instructions, causing the method described in any one of claims 1 to 5 to be executed.

7. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when run on a processor, causes the processor to perform the method according to any one of claims 1 to 5.

Images