Inertial-based integrated navigation system error state model correction and optimization method and system

Abstract

The application discloses an inertial-based integrated navigation system error state model correction and optimization method and system, and belongs to the field of integrated navigation control. In a local integrated navigation coordinate system, the influence of misalignment angles and position error angles on the mismatch problem of a calculated navigation coordinate system and a real navigation coordinate system is comprehensively considered, a nonlinear velocity error state corrected by misalignment angles and position error angles is redefined, and a new inertial-based integrated navigation system error state model is derived and established. Compared with the ST-EKF, the application not only considers the influence of linearization errors caused by misalignment angles, but also adds a correction term of the position error angles, further perfects the inertial-based integrated navigation system error state model optimization theory, effectively improves the performance and environmental applicability of state estimation of the inertial-based integrated navigation system, and the superiority is more significant especially in the case that the initial state error is large.

Description

Technical Field

[0001] This invention belongs to the field of integrated navigation control, and more specifically, relates to a method and system for correcting and optimizing the error state model of an inertial-based integrated navigation system. Background Technology

[0002] To address the linearization error problem in traditional integrated navigation system error state models, ST-EKF has attracted 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 error (i.e., misalignment angle), redefines the nonlinear velocity error state, and exhibits better estimation performance than traditional filtering algorithms under large misalignment angle conditions.

[0003] However, the navigation coordinate system is established with the observer's position coordinates as the origin, and the calculated navigation coordinate system is based on the output position of the SINS (Strapdown Inertial Navigation System). Therefore, position error angles will also cause the calculated navigation coordinate system to deviate from the true navigation coordinate system. Especially when the position error is large, it will adversely affect the state estimation performance of the integrated navigation system. Summary of the Invention

[0004] In view of the shortcomings of the prior art, the purpose of this invention is to provide a method and system for correcting and optimizing the error state model of an inertial-based integrated navigation system, aiming to solve the problem that the performance of the integrated navigation system state estimation is adversely affected when the prior art is applicable to situations with large position errors.

[0005] To achieve the above objectives, in a first aspect, the present invention provides a method for correcting and optimizing the error state model of an inertial-based integrated navigation system, comprising:

[0006] S1. Obtain the error state and error state model of the KF-based inertial-based integrated navigation system. The error state includes misalignment angle, velocity error and position error. The error state model includes attitude error equation, velocity error equation and position error equation.

[0007] S2. Convert the position error into a position error angle, and use the misalignment angle and the position error angle to correct the velocity error, thereby obtaining the corrected nonlinear velocity error state;

[0008] S3. Based on the corrected nonlinear velocity error state, the error state model of the integrated navigation system is corrected and optimized.

[0009] Preferably, the optimized nonlinear velocity error state δv γ It is expressed as follows:

[0010]

[0011] in,

[0012]

[0013] θ=N4δp

[0014]

[0015] δv n For the velocity error state based on KF, v n Let φ be the velocity of the carrier in the navigation system, the superscript ~ indicates the estimate of the corresponding variable, φ be the misalignment angle, θ be the position error angle, (·×) be the antisymmetric matrix operator, N4 be the intermediate matrix with no actual physical meaning, δp be the position error state, and L be the latitude.

[0016] Preferably, the optimized attitude error equation is expressed as follows:

[0017]

[0018] in,

[0019]

[0020]

[0021]

[0022]

[0023] The superscript · indicates the first derivative; N1, N2, N3, and N4 are the first to fourth intermediate matrices, respectively, and have no actual physical meaning; v n Let represent the velocity of the vehicle in the navigation system. The superscript ~ indicates the estimate of the corresponding variable, and (·×) is the antisymmetric matrix operator. Let φ be the angular velocity of the navigation frame relative to the inertial frame, φ be the misalignment angle, and δv be the angular velocity of the navigation frame relative to the inertial frame. γ The corrected nonlinear velocity error state is represented by δp, which represents the position error state. The measurement error is represented by gyroscope measurement, where L is latitude, h is altitude, and R is distance. M R is the principal curvature radius of the meridian. N Let ω be the principal radius of curvature of the zonal loop. ie v is the angular velocity of Earth's rotation. E Let v be the eastward velocity. N The speed is northbound.

[0024] Preferably, the optimized velocity error equation is expressed as follows:

[0025]

[0026] in,

[0027]

[0028]

[0029]

[0030]

[0031] The superscript · indicates the first derivative; N1, N2, N3, and N4 are intermediate matrices with no actual physical meaning; v n Let represent the velocity of the vehicle in the navigation system. The superscript ~ indicates the estimate of the corresponding variable, and (·×) is the antisymmetric matrix operator. This is the projection of the Earth's rotational angular velocity onto the navigation system. Let φ be the projection of gravity onto the navigation frame, and φ be the misalignment angle. Let δv be the projection of the angular velocity of the navigation frame relative to the Earth frame onto the navigation frame. γ This is the corrected nonlinear velocity error state. Let δp be the transformation matrix from the navigation system to the carrier system, and δp be the position error state. For comparison, For gyroscope measurement error, The specific force error measured by the accelerometer, where L is latitude, h is altitude, and R is... M R is the principal curvature radius of the meridian. N Let ω be the principal radius of curvature of the zonal loop. ie v is the angular velocity of Earth's rotation. E Let v be the eastward velocity. N The speed is northbound.

[0032] Preferably, the corrected position error equation is expressed as follows:

[0033]

[0034] in,

[0035]

[0036]

[0037]

[0038] The superscript · indicates the first derivative, N pv N4, N pp This is an intermediate matrix with no actual physical meaning; v n Let δv be the velocity of the vehicle in the navigation system. The superscript ~ indicates the estimate of the corresponding variable. (·×) is the antisymmetric matrix operator.γ The corrected nonlinear velocity error state is given by δp, the position error state is given by L, the latitude is given by h, and R is given by R. M R is the principal curvature radius of the meridian. N Let ω be the principal radius of curvature of the zonal loop. ie v is the angular velocity of Earth's rotation. E Let v be the eastward velocity. N The speed is northbound.

[0039] To achieve the above objectives, in a second aspect, the present invention provides an error state model correction and optimization system for an inertial-based integrated navigation system, 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, causing the method described in the first aspect to be executed.

[0040] 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.

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

[0042] This invention provides a method and system for correcting and optimizing the error state model of an inertial-based integrated navigation system. In the local integrated navigation coordinate system, it comprehensively considers the influence of misalignment angles and position error angles on the mismatch between the calculated and actual navigation coordinate systems. It redefines the nonlinear velocity error state after correction for misalignment and position error angles. Based on this, a new error state model for the inertial-based integrated navigation system is derived. Compared to ST-EKF, this invention not only considers the influence of linearization errors caused by misalignment angles but also adds a correction term for position error angles, further improving the optimization theory of the error state model for inertial-based integrated navigation systems. This effectively enhances the performance and environmental applicability of the inertial-based integrated navigation system state estimation, especially when the initial state error is large, where its superiority is even more significant. Attached Figure Description

[0043] Figure 1 This is a flowchart of a method for correcting and optimizing the error state model of an inertial-based integrated navigation system provided by the present invention. Detailed Implementation

[0044] 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.

[0045] like Figure 1 As shown, this invention provides a method for correcting and optimizing the error state model of an inertial-based integrated navigation system, comprising:

[0046] S1. Obtain the error state and error state model of the KF-based inertial-based integrated navigation system. The error state includes misalignment angle, velocity error and position error. The error state model includes attitude error equation, velocity error equation and position error equation.

[0047] S2. Convert the position error into a position error angle, and use the misalignment angle and position error angle to correct the velocity error, thereby obtaining the corrected nonlinear velocity error state.

[0048] The nonlinear velocity error, which combines the corrections for the misalignment angle and the position error angle, can be expressed as:

[0049]

[0050] in, This is the transformation matrix from the navigation coordinate system to the Earth coordinate system. Let v be the transformation matrix from the navigation system to the carrier system. n Let be the velocity of the carrier (e.g., a ship) in the navigation system. The superscript ~ indicates the estimate of the corresponding variable. When the misalignment angle φ and position error angle θ are considered as small angles, the following conditions are met:

[0051]

[0052]

[0053] θ=N4δp

[0054] Where, δv n This represents the traditional velocity error state, (·×) represents the antisymmetric matrix operator, and I is a 3×3 identity matrix.

[0055] S3. Based on the corrected nonlinear velocity error state, the error state model of the integrated navigation system is corrected and optimized.

[0056] The present invention specifies the following method for calculating the correlation matrix:

[0057]

[0058]

[0059]

[0060]

[0061]

[0062]

[0063] Where L is the latitude of the carrier, h is the height of the carrier, and R is the height of the carrier. M R is the principal curvature radius of the meridian. N Let ω be the principal radius of curvature of the zonal loop. ie v is the angular velocity of Earth's rotation. E Let v be the eastward velocity of the carrier. N The northward velocity of the carrier.

[0064] I. Derivation of the Attitude Error Equation

[0065]

[0066] in, Let be the angular velocity of the navigation frame relative to the inertial frame. The value represents the gyroscope measurement error, δp represents the position error state, and the superscript · indicates the first derivative.

[0067] II. Derivation of the Velocity Error Equation

[0068]

[0069] in, This is the projection of the Earth's rotational angular velocity onto the navigation system. Let be the projection of the angular velocity of the navigation frame relative to the Earth frame onto the navigation frame. For comparison, The specific force error measured by the accelerometer. This is the projection of gravity onto the navigation system.

[0070] III. Derivation of the Position Error Equation

[0071]

[0072] This invention is applicable to the Northeast-Eastern Sky Navigation Coordinate System and the Northeast-Eastern Earth Coordinate System.

[0073] 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 correcting and optimizing the error state model of an inertial-based integrated navigation system, characterized in that, include: S1. Obtain the error state and error state model of the KF-based inertial-based integrated navigation system. The error state includes misalignment angle, velocity error and position error. The error state model includes attitude error equation, velocity error equation and position error equation. S2. Convert the position error into a position error angle, and use the misalignment angle and the position error angle to correct the velocity error, thereby obtaining the corrected nonlinear velocity error state; S3. Based on the corrected nonlinear velocity error state, the error state model of the integrated navigation system is corrected and optimized; Optimized nonlinear velocity error state It is expressed as follows: in, For the velocity error state based on KF, The speed of the carrier in the navigation system, indicated by the superscript. This represents the estimate of the corresponding variable. For misalignment angle, The position error angle, For antisymmetric matrix operators, This is the fourth intermediate matrix, which has no actual physical meaning. This is the position error state. Latitude.

2. The method as described in claim 1, characterized in that, The optimized attitude error equation is expressed as follows: in, Superscript denotes the first-order derivative, , , are the first to third intermediate matrices respectively, without actual physical meaning, is the angular velocity of the navigation system relative to the inertial system, is the corrected non-linear velocity error state, is the transformation matrix from the navigation system to the vehicle system, is the gyro measurement error, is the altitude, is the radius of curvature of the prime vertical of the meridian, is the radius of curvature of the prime vertical of the prime vertical, is the angular velocity of the Earth's rotation, is the eastward velocity, is the northward velocity.

3. The method as described in claim 1, characterized in that, The optimized velocity error equation is expressed as follows: in, Superscript represents the first-order derivative, , , are the first to third intermediate matrices respectively, without actual physical meaning, is the projection of the earth's angular velocity of rotation in the navigation system, is the projection of gravity in the navigation system, is the projection of the angular velocity of the navigation system relative to the earth system in the navigation system, is the corrected non-linear velocity error state, is the transformation matrix from the navigation system to the vehicle body system, is the specific force, is the gyro measurement error, is the specific force error measured by the accelerometer, is the altitude, is the radius of curvature of the prime vertical, is the radius of curvature of the卯酉圈 (it seems there might be a typo here, perhaps it should be something like "prime vertical circle" or a correct term), is the earth's angular velocity of rotation, is the eastward velocity, is the northward velocity.

4. The method as described in claim 1, characterized in that, The corrected position error equation is expressed as follows: in, Superscript represents the first derivative, , is an intermediate matrix without actual physical meaning, is the corrected non - linear velocity error state, is the altitude, is the prime radius of curvature of the meridian, is the prime radius of curvature of the prime vertical, is the angular velocity of the Earth's rotation, is the eastward velocity, is the northward velocity.

5. A system for correcting and optimizing the error state model of an inertial-based integrated navigation system, 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 4 to be executed.

6. 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 4.

Images