Fusion navigation method based on adaptive dual set membership strategy and related product

Abstract

The invention discloses a fusion navigation method based on a self-adaptive dual set membership strategy and a related product, and belongs to the technical field of satellite navigation. According to the fusion navigation method based on the adaptive dual set membership strategy provided by the invention, a dual set membership filter is combined with a Frank-Wolfe algorithm, and the minimum outsourcing ellipsoid of a state after nonlinear transformation is directly calculated through a semi-infinite programming technology, so that the ellipsoid boundary of state estimation can more closely surround a real state, and the navigation precision is improved; a self-adaptive measurement variance weight adjustment mechanism based on an index residual error is introduced, a measurement noise covariance matrix is dynamically adjusted according to a real-time measurement residual error, the influence of an abnormal observation value is effectively inhibited, the filtering stability is kept in an unknown or time-varying noise environment, and the reliability of a navigation system in a complex environment is greatly improved.

Description

Technical Field

[0001] This invention relates to the field of satellite navigation technology, specifically to a fusion navigation method and related products based on an adaptive dual-member strategy. Background Technology

[0002] Existing inertial / satellite (INS / GPS) integrated navigation systems mainly employ two modes: loose combination and tight combination. The loose combination model is primarily based on the simple superposition of position and velocity information, while the tight combination model, through deep fusion of pseudorange and pseudorange rate, can maintain high navigation accuracy even with a smaller number of satellites.

[0003] However, due to the complexity and variability of the navigation environment, the tightly coupled INS / GPS integrated navigation model often has nonlinear and uncertain characteristics, which makes traditional filtering methods, such as unscented Kalman filtering (UKF) and commensurate Kalman filtering (CKF), insufficient in accuracy when dealing with high-dimensional nonlinear systems.

[0004] Meanwhile, existing filtering methods often require assumptions about the statistical characteristics of noise when dealing with unknown but bounded noise. This may lead to a decrease in filtering performance in practical applications, especially when the statistical characteristics of the state disturbances and measurement noise of dynamic systems are not fully known or difficult to model accurately. In such cases, traditional filtering methods cannot provide reliable estimation results.

[0005] Therefore, improving the navigation accuracy and reliability of inertial / satellite tightly coupled navigation systems has become a technical challenge that urgently needs to be overcome by those skilled in the art. Summary of the Invention

[0006] The purpose of this invention is to provide a fusion navigation method and related products based on an adaptive dual-member strategy, so as to overcome the problems of insufficient navigation accuracy and reliability of existing inertial / satellite tightly coupled navigation systems.

[0007] The present invention solves the above-mentioned technical problems through the following technical solution: This invention provides a fusion navigation method based on an adaptive dual set member strategy, comprising the following steps: S1. Construct the state equations and measurement equations for the tightly integrated INS / GPS navigation system; S2. Based on the exponential residual algorithm, adaptively adjust the measurement noise covariance matrix; update the measurement equation based on the adjusted measurement noise covariance matrix. S3. Based on the dual set member filter, the Frank-Wolfe algorithm is used to calculate the predicted ellipsoid of the state vector in the current state equation; combined with the updated measurement equation, the Frank-Wolfe algorithm is used to calculate the measured ellipsoid of the state vector in the current state equation; the predicted ellipsoid and the measured ellipsoid intersect to obtain the updated ellipsoid; based on the updated ellipsoid, the state equation is updated. S4. Obtain the inertial navigation error of the current INS / GPS tightly integrated navigation system, and determine whether the inertial navigation error is greater than the preset error. If the determination result is yes, return to step S3; if the determination result is no, output navigation information based on the current INS / GPS tightly integrated navigation system.

[0008] A further improvement of the present invention is that step S1 specifically includes the following steps: Using the attitude error, velocity error, position error, gyro drift error, and accelerometer bias error of the INS as state variables, the state equation of the INS / GPS tightly integrated navigation system is constructed. The process noise of the state equation satisfies the ellipsoidal constraint. Using the difference between the pseudorange calculated by the INS and the pseudorange measured by the GPS receiver as the observation, the measurement equation of the INS / GPS tightly integrated navigation system is constructed. The measurement noise of the measurement equation satisfies the ellipsoidal constraint.

[0009] A further improvement of this invention is that the state equation is:

[0010] in, For state vectors, These represent the attitude errors in the east, north, and sky directions, respectively. The velocity errors are respectively in the east, north, and sky directions; These are the positional errors in latitude, longitude, and altitude, respectively. These represent the gyroscope drift errors on the x-axis, y-axis, and z-axis, respectively. These represent the zero bias errors of the accelerometers on the x-axis, y-axis, and z-axis, respectively. This is the state transition matrix; This is process noise; Let k be the state vector at time k; Let k be the state vector at time k-1; The measurement equation is:

[0011] in, For measurement equations; For the observation model function; For measuring noise; A further improvement of this invention is that the adaptive adjustment of the measurement noise covariance matrix based on exponential residuals specifically includes the following steps: based on Time-state volume point ,calculate Predicting volume points based on time-state; based on The state prediction volume point at each time step is used to calculate the predicted pseudorange measurement value and obtain the pseudorange information for the current time step. Based on the pseudorange information at the current time step, adaptive weights are introduced to adjust the measurement noise covariance matrix.

[0012] A further improvement of this invention is that the adaptive weights are specifically as follows:

[0013] in, For adaptive weights; This is the pseudo-distance information for the current time step; This is an adjustable parameter with a value range of 0 to 1, used to control the attenuation coefficient.

[0014] A further improvement of this invention is that the prediction ellipsoid of the state vector in the current state equation is specifically:

[0015]

[0016] in, The center of the predicted ellipsoid for the state vector in the current state equation; The center of the ellipsoid of the set after the nonlinear transformation of the state variables; The current state matrix of the predicted ellipsoid is the state vector in the current state equation. The ellipsoidal shape matrix of the set after nonlinear transformation of state variables; These are the parameters used to minimize the predicted ellipsoid size; The ellipsoidal shape matrix represents the process noise; The measurement ellipsoid for the state vector in the current state equation is specifically:

[0017]

[0018] in, The shape matrix of the ellipsoid measured at the current time k; The center of the ellipsoid is measured at the current time k. It is a set of state variables; To find the determinant function of a matrix; To find the logarithmic function; The updated ellipsoid is obtained by intersecting the predicted ellipsoid and the measured ellipsoid.

[0019]

[0020] in, The center of the updated ellipsoid; To update the size weight parameters of the ellipsoid; To select the projection matrix for the parameters; To select the transpose of the projection matrix; For measuring the shape matrix of the ellipsoid; To measure the center of the ellipsoid.

[0021] This invention also provides a fusion navigation system based on an adaptive dual set member strategy, comprising: The first module is used to construct the state equations and measurement equations of the tightly integrated INS / GPS navigation system; The second module is used to adaptively adjust the measurement noise covariance matrix based on the exponential residual; and to update the measurement equation based on the adjusted measurement noise covariance matrix. The third module is used to calculate the predicted ellipsoid of the state vector in the current state equation based on the dual set member filter and the Frank-Wolfe algorithm; combined with the updated measurement equation, the measured ellipsoid of the state vector in the state equation is calculated using the Frank-Wolfe algorithm; the predicted ellipsoid and the measured ellipsoid are intersected to obtain the updated ellipsoid; and the state equation is updated based on the updated ellipsoid. The fourth module is used to obtain the inertial navigation error of the current INS / GPS tightly coupled navigation system, and output navigation information when the inertial navigation error is less than the preset error.

[0022] The present invention also provides a computer device, including a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the steps of the fusion navigation method based on the adaptive dual set member strategy described above.

[0023] The present invention also provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the steps of the fusion navigation method based on the adaptive dual set member strategy described above.

[0024] The present invention also provides a computer program product, including a computer program that, when executed by a processor, implements the steps of the fusion navigation method based on the adaptive dual set member strategy described above.

[0025] Compared with the prior art, the positive and progressive effects of the present invention are as follows: The fusion navigation method based on an adaptive dual-member strategy provided by this invention employs a dual-member filter combined with the Frank-Wolfe algorithm. It directly calculates the minimum enclosing ellipsoid of the nonlinearly transformed state through semi-infinite programming, enabling the ellipsoid boundary of the state estimation to more tightly enclose the real state, thereby improving navigation accuracy. Furthermore, it introduces an adaptive measurement variance weight adjustment mechanism based on exponential residuals, which dynamically adjusts the measurement noise covariance matrix according to the real-time measurement residuals. This effectively suppresses the influence of abnormal observations and maintains filtering stability in unknown or time-varying noise environments, significantly improving the reliability of the navigation system in complex environments. Attached Figure Description

[0026] The accompanying drawings are provided to further understand the invention and constitute a part of this invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an improper limitation of the invention.

[0027] Figure 1 This is a block diagram of a fusion navigation system based on an adaptive dual set member strategy according to the present invention.

[0028] Figure 2 This is a diagram showing the effect of positional error.

[0029] Figure 3 This is a diagram showing the upper and lower bounds of the ellipsoid, including positional errors. Detailed Implementation

[0030] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0031] In the description of this invention, it should be understood that the terms "comprising" and "including" indicate the presence of the described features, integrals, steps, operations, elements and / or components, but do not exclude the presence or addition of one or more other features, integrals, steps, operations, elements, components and / or collections thereof.

[0032] It should also be understood that the terminology used in this specification is for the purpose of describing particular embodiments only and is not intended to limit the invention. As used in this specification and the appended claims, the singular forms “a,” “an,” and “the” are intended to include the plural forms unless the context clearly indicates otherwise.

[0033] It should be understood that although terms such as first, second, third, etc., may be used in the embodiments of the present invention to describe the preset range, these preset ranges should not be limited to these terms. These terms are only used to distinguish the preset ranges from one another. For example, without departing from the scope of the embodiments of the present invention, the first preset range may also be referred to as the second preset range, and similarly, the second preset range may also be referred to as the first preset range.

[0034] Depending on the context, the word "if" as used here can be interpreted as "when," "when," "in response to determination," or "in response to detection." Similarly, depending on the context, the phrase "if determination" or "if detection (of the stated condition or event)" can be interpreted as "when determination," "in response to determination," "when detection (of the stated condition or event)," or "in response to detection (of the stated condition or event)."

[0035] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. This is an explanation of the present invention and not a limitation thereof.

[0036] The INS / GPS tightly integrated model is a deeply integrated navigation technology that combines the advantages of the Global Positioning System (GPS) and the Inertial Navigation System (INS), overcoming the limitations of using them individually and providing higher accuracy and reliability for the navigation system. This model is characterized by utilizing pseudorange and pseudorange rate information provided by GPS, combined with inertial measurement data from INS, to achieve high-precision integrated navigation.

[0037] This invention provides a fusion navigation method based on an adaptive dual set member strategy, comprising the following steps: S1. Construct the state equations and measurement equations for the tightly integrated INS / GPS navigation system; S2. Based on the exponential residual algorithm, adaptively adjust the measurement noise covariance matrix; update the measurement equation based on the adjusted measurement noise covariance matrix. S3. Based on the dual set member filter, the Frank-Wolfe algorithm is used to calculate the predicted ellipsoid of the state vector in the current state equation; combined with the updated measurement equation, the Frank-Wolfe algorithm is used to calculate the measured ellipsoid of the state vector in the current state equation; the predicted ellipsoid and the measured ellipsoid intersect to obtain the updated ellipsoid; based on the updated ellipsoid, the state equation is updated. S4. Obtain the inertial navigation error of the current INS / GPS tightly integrated navigation system, and determine whether the inertial navigation error is greater than the preset error. If the determination result is yes, return to step S3; if the determination result is no, output navigation information based on the current INS / GPS tightly integrated navigation system.

[0038] The fusion navigation method based on an adaptive dual-member strategy provided by this invention employs a dual-member filter combined with the Frank-Wolfe algorithm. It directly calculates the minimum enclosing ellipsoid of the nonlinearly transformed state through semi-infinite programming, enabling the ellipsoid boundary of the state estimation to more tightly enclose the real state, thereby improving navigation accuracy. Furthermore, it introduces an adaptive measurement variance weight adjustment mechanism based on exponential residuals, which dynamically adjusts the measurement noise covariance matrix according to the real-time measurement residuals. This effectively suppresses the influence of abnormal observations and maintains filtering stability in unknown or time-varying noise environments, significantly improving the reliability of the navigation system in complex environments.

[0039] See Figure 1 In a specific embodiment of the present invention, step S1 specifically includes: The attitude error, velocity error, position error, gyroscope drift error, and accelerometer bias error of the INS are selected as state variables. The state equation of the navigation system can be written as: (1) For state vectors, These represent the attitude errors in the east, north, and sky directions, respectively. The velocity errors are respectively in the east, north, and sky directions; These are the positional errors in latitude, longitude, and altitude, respectively. These represent the gyroscope drift errors on the x-axis, y-axis, and z-axis, respectively. These represent the zero bias errors of the accelerometers on the x-axis, y-axis, and z-axis, respectively. This is the state transition matrix; This is process noise; Let k be the state vector at time k; Let be the state vector at time k-1.

[0040] Process noise Satisfying ellipsoidal constraints: (2) in, The set to which the noise belongs; For noise ellipsoid; This is the transpose of the noise ellipsoid; This is the process noise shape matrix.

[0041] The compact combination model uses satellite ephemeris data provided by a GPS receiver and position and velocity data provided by an INS to calculate the pseudorange corresponding to the INS position and velocity. ,Will Measurements with GPS receiver In contrast to observations, the error quantities of the INS and GPS receivers are estimated by combining filters.

[0042] The inertial navigation output position of the carrier in the Earth's geodetic coordinate system is expressed as longitude. ,latitude ,high The true location of the carrier is represented as , , , Indicates the error value; This is due to longitude error; This is due to latitude error; This is for height error; Converting it to the Geocentric Geofixed (ECEF) coordinate system is as follows: (3) (4) (5) in, For the Earth's radius, Earth's eccentricity; These are the three-dimensional position coordinates in the geocentric fixed system.

[0043] The actual distance from the satellite to the carrier (j=1,2,3) is represented as: (6) in, These are the satellite's position coordinates.

[0044] The pseudorange between satellites j measured by the GPS receiver is expressed as: (7) In the formula, (j=1,2,3) represents the pseudorange from the GPS receiver to the three satellites. This is noise in pseudorange measurement.

[0045] The difference between the pseudorange calculated by INS and the pseudorange measured by the GPS receiver is used as the observation: (8) That is, the corresponding representation is: (9) in, For measurement equations; For the observation model function; To measure noise, the ellipsoidal constraint must be satisfied: (10) in, To measure the noise covariance matrix, To include the measurement noise range; For measuring noise; This is the transpose representation of the measured noise; The derivative of the noise covariance matrix is ​​used to measure the noise level.

[0046] In tightly integrated INS / GPS navigation systems, GPS measurement information is susceptible to external signal interference and environmental factors (such as obstruction and multipath effects), leading to measurement anomalies and resulting in random variations in measurement noise variance. To overcome these anomalies, this invention proposes an adaptive noise variance weighting mechanism for navigation systems based on exponential residuals. This mechanism dynamically adjusts the noise covariance matrix to improve the robustness and accuracy of filtering.

[0047] In a specific embodiment of the present invention, step S2 specifically includes: Adaptive Measurement Noise Variance Weighting Mechanism for Navigation Systems Based on Exponential Residues 1. Calculation Time-state volume point The probability distribution used to approximate the navigation state: (11) in: The first generation matrix of the volume points Columns, matrices are defined as follows: (12) for The time-state error covariance matrix, Let be the state vector at time k-1.

[0048] 2. According to Time-state volume point ,calculate Time-based navigation status prediction: (13) in, The normalized weight of the i-th volume point; This represents the total number of volume points generated. 3. According to Time-of-flight navigation state prediction and The state volume point at time step, calculation Time-state prediction covariance matrix: (14) in, The process noise covariance matrix can be estimated using the maximum likelihood estimation method.

[0049] 4. According to Time-of-flight navigation state prediction and The covariance matrix of state prediction at each time step is calculated. Predicted volumetric points for time-state conditions: (15) in, The current state matrix of the predicted ellipsoid is the state vector in the current state equation. 5. Utilize Predict the volumetric point based on the state at each time point, and calculate the predicted pseudorange measurement value: (16) 6. Obtain the measurement at the current time step through the measurement equation. Then, the pseudorange information (observation residual) at the current time step is calculated: (17) in, The observation matrix; This is the pseudo-distance information for the current time step.

[0050] 7. Introduce adaptive weights Adjust the measurement noise covariance matrix This is used to control the update rate of the covariance matrix. (18) in, The attenuation coefficient is used to control the dynamic adjustment process of the measurement noise covariance matrix. Its main function is to smooth the changes in measurement noise and avoid the sharp decline in filter performance caused by sudden measurement anomalies. By introducing the attenuation coefficient, the measurement noise covariance matrix can be adaptively adjusted so that it can better adapt to the dynamically changing noise environment.

[0051] The formula for calculating the attenuation coefficient is as follows: (19) in, This is an adjustable parameter used to control the decay rate, and its value typically ranges from 0 to 1. When When the value is small, the attenuation coefficient is close to 1, indicating that the adjustment of the measurement noise covariance matrix is ​​relatively slow; when... When the value is large, the attenuation coefficient approaches 0, indicating that the adjustment of the measurement noise covariance matrix is ​​relatively rapid. In practical navigation applications, The value can be adjusted based on the dynamic characteristics of the environment. For example, in environments with severe signal obstruction and drastic noise changes, a larger value can be selected. Value; In open sky conditions, noise levels are relatively stable, so a lower value can be selected. value.

[0052] 8. Adjust the measurement noise covariance matrix as follows: (20) Updated Continue to satisfy the ellipsoidal constraints for subsequent filtering calculations.

[0053] 9. Update the measurement equation based on the adjusted measurement noise covariance matrix.

[0054] After completing the design of the navigation system model and the measurement variance weight adjustment mechanism, an INS / GPS compact combination navigation system based on Adaptive Dual Set-Membership Filter (ADSMF) can be further constructed. This involves adding an adaptive approach to the existing dual set-membership filter method to achieve accurate navigation state estimation. These errors are then fed back to the INS to correct its navigation output, thereby improving the accuracy and reliability of the entire navigation system.

[0055] In a specific embodiment of the present invention, step S3 specifically includes: I. Prediction Steps: To predict the navigation system state at the next time step, the navigation system first uses an ADSMF filter to calculate an ellipsoid. To tightly constrain the transformed set .

[0056] This is achieved by solving a semi-infinite programming problem: (twenty one) Constrained by: (twenty two) in, To estimate the center of the ellipsoid, The set of state ellipsoids from the previous time step. For ellipsoid The shape matrix, This is the state vector after nonlinear transformation.

[0057] By employing semi-infinite programming techniques, the linearization of nonlinear functions is avoided, and the minimum ellipsoid capable of covering the state after nonlinear transformation is directly calculated. Simultaneously, an adaptive measurement variance weighting mechanism based on exponential residuals is introduced to enhance robustness. This improves the accuracy and reliability of the filtering.

[0058] The Frank-Wolfe (FW) method is a first-order optimization algorithm that iteratively updates the solution to minimize the objective function. It solves this type of semi-infinite programming problem through the following steps: 1. Initialization: Select an initial feasible set. Including the center of the ellipsoid and initial shape matrix

[0059] 2. Multiple iterations: Calculate the gradient: , Let be the objective function. To find the gradient direction, which indicates the direction of the steepest descent of the objective function; to find the optimal direction, that is, to find the direction of the vertex that maximizes the linear approximation: ,in, A vertex in the feasible set of the ellipsoid, from the current point Pointing to the vertex The direction vector. This step uses linear approximation to find the vertex direction that maximizes the objective function value, thus determining the optimization direction.

[0060] Update step size : , This is the step size factor, usually determined through a line search, and its value ranges from [0,1]. Control the magnitude of updates to ensure that the iterative process converges to the optimal solution.

[0061] 3. Repeat the iteration until the convergence constraint is met, and output the converged ellipsoid center. and shape matrix Resulting in an ellipsoid Then, predict the ellipsoid set. Using Minkowski and To approximate.

[0062] The calculation of the predicted ellipsoid center and shape matrix is ​​as follows: (twenty three) (twenty four) in, The parameters used to minimize the predicted ellipsoid size are... Let be the ellipsoidal shape matrix of the process noise.

[0063] II. Measurement Update: Combined measurement equation Solving the problem using semi-infinite programming techniques, which includes the transformed measurement set. Minimum ellipsoid constraint: (25) Constraints (Pseudorange Observation Constraints): (26) in, The shape matrix of the ellipsoid at the current time k can be used as the measurement prediction matrix; The center of the ellipsoid is measured at the current time k. It is a set of state variables; To find the determinant function of a matrix; To find the logarithmic function.

[0064] Predicting the ellipsoid With measuring ellipsoid The intersection yields the updated ellipsoid The updated formulas for calculating the ellipsoid center and shape matrix are as follows: Ellipsoid Update: (27) (28) The intermediate variables (weight structure) used for the minimum bounding ellipsoid are calculated as follows: (29) (30) in, To optimize the weighting parameters for the ellipsoid size.

[0065] In a specific embodiment of the present invention, step S4 specifically includes: In the closed-loop calibration process of the navigation system, the output of the inertial navigation system (INS) is subtracted from the state estimate to obtain the inertial navigation error. This error is compensated through a real-time feedback correction mechanism, and finally, the corrected high-precision navigation information is output, ensuring that the accumulated error of the INS is dynamically eliminated, thereby improving the overall positioning accuracy and reliability.

[0066] This process achieves deep integration of INS and GPS through a closed-loop mechanism of "estimation-feedback-resolution-reset," enabling system self-calibration without external assistance. Specifically, the DSMF filter estimates the errors of the INS and GPS receivers at each time step. These errors include attitude error, velocity error, position error, and sensor bias error. The INS adjusts its attitude angle based on the feedback attitude error, corrects the velocity estimate based on the velocity error, corrects the position information based on the position error, and corrects the bias of the gyroscope and accelerometer. The error parameters estimated by the DSMF filter are also used to correct the pseudorange and pseudorange rate measurement errors of the GPS receiver, improving the accuracy of GPS positioning. In this way, the errors of the INS and GPS receivers are effectively estimated and corrected, thereby improving the accuracy and reliability of the entire navigation system.

[0067] To verify the effectiveness of the method of the present invention, an INS / GPS tightly coupled navigation system was constructed using the method of the present invention, which integrates the original GPS observation data and the error characteristics of INS inertial navigation, thus proving the effectiveness of the dual set member method.

[0068] The system utilizes pseudorange measurement data provided by a GPS receiver and position and velocity information of the inertial navigation system (INS). A nonlinear filtering model incorporating a 15-dimensional state vector and ellipsoidal constraint noise is then constructed. A dual set-member filter is employed to process these observations and estimate the errors of the INS and GPS receivers. This filter avoids linearizing the nonlinear function using semi-infinite programming techniques, directly calculating the minimum ellipsoid capable of covering the state after the nonlinear transformation. An adaptive measurement variance weighting mechanism based on exponential residuals is introduced to enhance robustness. Finally, the INS error estimate output by the ADSMF is used to correct the inertial navigation solution in real time via a closed-loop feedback mechanism, ultimately outputting high-precision navigation information.

[0069] See Figure 2 The graph shows the variation in longitude, latitude, and altitude positions. As can be seen, the errors were effectively controlled and converged during system operation. (See also...) Figure 3 Furthermore, it demonstrates the upper and lower bounds of the ellipsoid that include these errors, intuitively reflecting the superiority of the adaptive dual set member filtering method in uncertainty estimation.

[0070] Compared to traditional INS / GPS tightly coupled navigation systems based on UKF and CKF, the ADSMF-based INS / GPS tightly coupled navigation system achieves significant improvements in navigation and positioning accuracy and reliability. Specifically, the ADSMF filter effectively improves the accuracy of navigation output by more tightly enclosing the real state. Simultaneously, in unknown or changing noisy environments, the adaptive measurement variance weighting mechanism based on exponential residuals enhances the robustness and reliability of the navigation output.

[0071] Based on the same inventive concept, embodiments of this application provide a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the steps of a fusion navigation method based on an adaptive dual-member strategy. The memory may include main memory, such as high-speed random access memory, and may also include non-volatile memory, such as at least one disk storage device. The processor, network interface, and memory are interconnected via an internal bus, which may be an industry-standard architecture bus, a peripheral component interconnection standard bus, an extended industry-standard architecture bus, etc. The bus can be divided into an address bus, a data bus, a control bus, etc. The memory is used to store the program; specifically, the program may include program code, which includes computer operation instructions. The memory may include main memory and non-volatile memory, and provides instructions and data to the processor.

[0072] Based on the same inventive concept, embodiments of this application provide a computer-readable storage medium storing a computer program. When executed by a processor, the computer program implements the steps of the fusion navigation method based on an adaptive dual set member strategy. Specifically, the computer-readable storage medium includes, but is not limited to, volatile memory and / or non-volatile memory. The volatile memory may include RAM (Random Access Memory) and / or cache memory, etc. The non-volatile memory may include ROM (Read-Only Memory), hard disk, flash memory, optical disk, magnetic disk, etc.

[0073] Based on the same inventive concept, this application provides a computer program product, which includes a computer program stored on a computer-readable storage medium. The computer program includes program instructions that, when executed by a computer device, cause the computer device to perform the steps of the above-described fusion navigation method based on an adaptive dual set member strategy.

[0074] Those skilled in the art will understand that embodiments of the present invention can be provided as methods or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM (Compact Disc Read-Only Memory), optical storage, etc.) containing computer-usable program code.

[0075] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, as well as combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer apparatus or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0076] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer device or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0077] These computer program instructions may also be loaded onto a computer device or other programmable data processing equipment to cause a series of operational steps to be performed on the computer device or other programmable equipment to produce a process implemented by the computer device, thereby providing instructions that execute on the computer device or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0078] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments as well as all changes and modifications falling within the scope of the invention.

[0079] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, this invention also intends to include these modifications and variations.

Claims

1. A fusion navigation method based on an adaptive dual set member strategy, characterized in that, Includes the following steps: S1. Construct the state equations and measurement equations for the tightly integrated INS / GPS navigation system; S2. Based on the exponential residual algorithm, adaptively adjust the measurement noise covariance matrix; update the measurement equation based on the adjusted measurement noise covariance matrix. S3. Based on the dual set member filter, the Frank-Wolfe algorithm is used to calculate the predicted ellipsoid of the state vector in the current state equation; combined with the updated measurement equation, the Frank-Wolfe algorithm is used to calculate the measured ellipsoid of the state vector in the current state equation; the predicted ellipsoid and the measured ellipsoid intersect to obtain the updated ellipsoid; based on the updated ellipsoid, the state equation is updated. S4. Obtain the inertial navigation error of the current INS / GPS tightly integrated navigation system, and determine whether the inertial navigation error is greater than the preset error. If the determination result is yes, return to step S3. If the judgment result is negative, then navigation information is output based on the current INS / GPS tightly coupled navigation system.

2. The fusion navigation method based on an adaptive dual set member strategy according to claim 1, characterized in that, Step S1 specifically includes the following steps: Using the attitude error, velocity error, position error, gyro drift error, and accelerometer bias error of the INS as state variables, the state equation of the INS / GPS tightly integrated navigation system is constructed. The process noise of the state equation satisfies the ellipsoidal constraint. Using the difference between the pseudorange calculated by the INS and the pseudorange measured by the GPS receiver as the observation, the measurement equation of the INS / GPS tightly integrated navigation system is constructed. The measurement noise of the measurement equation satisfies the ellipsoidal constraint.

3. The fusion navigation method based on an adaptive dual set member strategy according to claim 2, characterized in that, The state equation is: in, For state vectors, These represent the attitude errors in the east, north, and sky directions, respectively. The velocity errors are respectively in the east, north, and sky directions; These are the positional errors in latitude, longitude, and altitude, respectively. These represent the gyroscope drift errors on the x-axis, y-axis, and z-axis, respectively. These represent the zero bias errors of the accelerometers on the x-axis, y-axis, and z-axis, respectively. This is the state transition matrix; This is process noise; Let k be the state vector at time k; Let k be the state vector at time k-1; The measurement equation is: in, For measurement equations; For the observation model function; For measuring noise.

4. The fusion navigation method based on an adaptive dual set member strategy according to claim 1, characterized in that, The adaptive adjustment of the measurement noise covariance matrix based on exponential residuals specifically includes the following steps: based on Time-state volume point ,calculate Predicting volume points based on time-state; based on The state prediction volume point at each time step is used to calculate the predicted pseudorange measurement value and obtain the pseudorange information for the current time step. Based on the pseudorange information at the current time step, adaptive weights are introduced to adjust the measurement noise covariance matrix.

5. The fusion navigation method based on an adaptive dual set member strategy according to claim 4, characterized in that, The adaptive weights are as follows: in, For adaptive weights; This is the pseudo-distance information for the current time step; This is an adjustable parameter with a value range of 0 to 1, used to control the attenuation coefficient.

6. The fusion navigation method based on an adaptive dual set member strategy according to claim 1, characterized in that, The prediction ellipsoid of the state vector in the current state equation is specifically as follows: in, The center of the predicted ellipsoid for the state vector in the current state equation; The center of the ellipsoid of the set after the nonlinear transformation of the state variables; The current state matrix of the predicted ellipsoid is the state vector in the current state equation. The ellipsoidal shape matrix of the set after nonlinear transformation of state variables; These are the parameters used to minimize the predicted ellipsoid size; The ellipsoidal shape matrix represents the process noise; The measurement ellipsoid for the state vector in the current state equation is specifically: in, The shape matrix of the ellipsoid measured at the current time k; The center of the ellipsoid is measured at the current time k. It is a set of state variables; To find the determinant function of a matrix; To find the logarithmic function; The updated ellipsoid is obtained by intersecting the predicted ellipsoid and the measured ellipsoid. in, The center of the updated ellipsoid; To update the size weight parameters of the ellipsoid; To select the projection matrix for the parameters; To select the transpose of the projection matrix; For measuring the shape matrix of the ellipsoid; To measure the center of the ellipsoid.

7. A fusion navigation system based on an adaptive dual set member strategy, characterized in that, include: The first module is used to construct the state equations and measurement equations of the tightly integrated INS / GPS navigation system; The second module is used to adaptively adjust the measurement noise covariance matrix based on the exponential residual. The measurement equation is updated based on the adjusted measurement noise covariance matrix. The third module is used to calculate the predicted ellipsoid of the state vector in the current state equation based on the dual set member filter and the Frank-Wolfe algorithm; combined with the updated measurement equation, the measured ellipsoid of the state vector in the state equation is calculated using the Frank-Wolfe algorithm; the predicted ellipsoid and the measured ellipsoid are intersected to obtain the updated ellipsoid; and the state equation is updated based on the updated ellipsoid. The fourth module is used to obtain the inertial navigation error of the current INS / GPS tightly coupled navigation system, and output navigation information when the inertial navigation error is less than the preset error.

8. A computer device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that, When the processor executes the computer program, it implements the steps of the fusion navigation method based on the adaptive dual set member strategy as described in any one of claims 1 to 6.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of the fusion navigation method based on the adaptive dual set member strategy as described in any one of claims 1 to 6.

10. A computer program product, comprising a computer program, characterized in that, When executed by a processor, the computer program implements the steps of the fusion navigation method based on the adaptive dual set member strategy as described in any one of claims 1 to 6.

Images