Multi-source heterogeneous robust navigation positioning method based on error state variation inference
By constructing a system state vector and observation model, and combining variational inference and ESKF, the accuracy and robustness problems caused by noise outliers in complex environments of traditional navigation methods are solved, and high-precision state estimation is achieved.
Patent Information
- Application Number
- CN202511755314.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-11-26
- Publication Date
- 2026-02-27
AI Technical Summary
Traditional integrated navigation methods are susceptible to interference from multipath effects, signal blockage, and strong motion shocks in complex environments, resulting in non-Gaussian distribution characteristics and outliers in observation noise, which reduces the accuracy and robustness of state estimation.
A system state vector is constructed, and a state propagation and observation model is established by combining the acceleration and angular velocity observations of INS, the position observations of GNSS, and the feature point observations of the camera. The observation noise of GNSS/INS is modeled as a Gaussian-Student's t mixture distribution, and variational inference and ESKF are used to jointly estimate the system state and noise parameters.
In the presence of outliers, the robustness and accuracy of system state estimation are improved, ensuring the accuracy and reliability of positioning.
Smart Images
Figure CN121577013A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to a multi-source heterogeneous robust navigation and positioning system based on error state variational inference. First, considering the actual operating scenario of the system, a system state vector is constructed. Then, based on INS acceleration and angular velocity observations, combined with ESKF, a system state propagation model is established. Considering INS orientation observations, GNSS position observations, and camera feature point observations, a system state observation model is established. Then, based on the actual observations of the integrated navigation system, the GNSS / INS observation noise is modeled as a Gaussian-Student's t mixture distribution. Variational inference techniques are used to obtain the approximate posterior distribution parameters of all latent variables in the probability model, achieving robust estimation of the system state. This improves the robustness and estimation accuracy of the algorithm when outliers exist in the process noise, and belongs to the field of automatic control. Background Technology
[0002] During autonomous navigation and positioning, the accuracy and robustness of state estimation directly impact the system's safety and reliability. Traditional integrated navigation methods typically rely on GNSS and INS information fusion and achieve state estimation through ESKF. However, real-world environments are complex and variable; sensor observations are susceptible to interference from multipath effects, signal blockage, and strong motion shocks, resulting in non-Gaussian distribution characteristics and outliers in the observation noise, thus reducing the estimation accuracy and robustness of traditional filtering methods. In recent years, multi-source information fusion based on visual sensors has become an important means to improve system positioning accuracy, ensuring system positioning accuracy even in scenarios where some sensors fail, and enhancing system reliability. However, how to achieve real-time and stable state estimation when GNSS / INS observations are abnormal remains a critical problem that urgently needs to be solved.
[0003] In recent years, robust state estimation has received widespread attention, and various estimation methods have been proposed. For example, a robust state estimation method based on dynamic scaling of the observation covariance matrix [Agarwal P, Tipaldi GD, Spinello L, et al. Robust map optimization using dynamic covariance scaling[C] / / 2013 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2013: 62-69.] is proposed. This scheme models the residuals between actual system observations and predictions, calculates the Mahalanobis distance based on a set covariance matrix, and constructs a dynamic scaling factor based on a set threshold. This scheme addresses the uncertainty of observation noise by introducing a dynamic scaling factor, amplifying the covariance matrix of anomalous observations, thereby eliminating the uncertainty of observation noise. However, this scheme does not consider the inherent characteristics of non-Gaussian noise, thus reducing the accuracy of state estimation. For example, the UAV localization method based on multi-sensor fusion [Zhao Zeyang. Robustness Study of UAV Localization Method Based on Multi-Sensor Fusion [D]. Jilin University, 2023.] aims to achieve system state estimation based on multiple INS and visual sensors. However, this scheme does not analyze the impact of sensor observation noise on the accuracy of system state estimation, especially lacking the analysis of abnormal noise in the system sensor reading signals, thus reducing the accuracy and robustness of system estimation. Summary of the Invention
[0004] Objective: To address the shortcomings of existing technologies, this invention provides a multi-source heterogeneous robust navigation and positioning method based on error state variational inference. First, considering the operational scenarios and requirements of the actual system, a system state is constructed. Then, based on acceleration and angular velocity observations from the Inertial Navigation System (INS), a system state propagation model is constructed. Subsequently, based on INS angle observations, feature point observations from camera sensors, and position observations from the Global Navigation Satellite System (GNSS), a state observation model is constructed. Finally, considering the outlier values in the observation noise of the integrated navigation GNSS / INS, the observation noise is modeled as a Gaussian-Student's t-mixture (GSTM) distribution, and a variational inference and error state Kalman filter (ErrorState Kalman Filter) are designed. This invention proposes a joint estimation scheme for system state and noise parameters of ESKF (Enhanced System State Function), which simultaneously estimates system state and noise parameters online. Using the system state model established in this invention, combined with a variational inference-based joint estimation method for GNSS / INS observation noise parameters and system state, the robustness and accuracy of system state estimation can be effectively improved even in the presence of significant outliers, thereby achieving precise positioning. This method is used to improve the positioning accuracy of the system when measurements contain outliers or some sensors fail.
[0005] Technical Solution: To achieve the above-mentioned objectives, the present invention provides a multi-source heterogeneous robust navigation and positioning method based on error state variational inference, comprising the following steps:
[0006] Step 1: Construct a system state vector based on the actual operating scenario of the system, and use ESKF to predict and update the system state based on sensor observations from INS, GNSS, and cameras. Specifically, this includes constructing a system error state propagation model using acceleration and angular velocity measurements from INS, and then establishing a sensor observation model based on GNSS position observations, camera feature point observations, and INS angle observations.
[0007] Step 2: Robust real-time estimation of system state and noise parameters based on variational inference. Specifically, this involves modeling GNSS / INS position and angle observation noise as GSTM distributions, constructing latent variables for noise parameters based on these distributions, and jointly estimating the probability distributions of system state and noise using variational inference and ESKF.
[0008] Preferably, step 1 specifically includes:
[0009] Step 11: Model the system state and construct the state propagation model;
[0010] Based on the actual operation scenario of the system, construct the real state of the system at discrete time points. nominal state and error state for
[0011]
[0012]
[0013]
[0014] in, , , These represent the system in the world coordinate system. The actual state, nominal state, and error state of the next position. , , Indicates the system in the world coordinate system The true state, nominal state, and error state of velocity at any given time. , , This represents the true, nominal, and error states of the system's orientation in the world coordinate system. , , This indicates the true state, nominal state, and error state of the acceleration zero bias of the system's INS. , , This indicates the true state, nominal state, and error state of the angular velocity zero bias of the system's INS. , , This represents the true, nominal, and error states of gravitational acceleration. The error state is determined by integrating the differential equation and combining it with acceleration observations from the INS. and angular velocity observation The propagation model of the systematic error state can be described as follows:
[0015]
[0016] in This indicates process noise disturbance. and These represent the state transition functions. For the Jacobian matrix of the error state and the perturbation, and Let these represent the predicted nominal state at the current time step and the updated nominal state at the previous time step, respectively. Indicates IMU observations;
[0017] Step 12, construct the camera observation model;
[0018] consider At this moment The observation of each feature point in the camera coordinate system is as follows:
[0019]
[0020]
[0021] in, This represents the feature point estimation results. and These represent the rotation and translation from the INS coordinate system to the camera coordinate system, respectively. The estimated value of the feature point in the world coordinate system can be approximated by deploying the least squares method. Based on the observed and estimated values of the feature point, the following observation residuals are constructed.
[0022]
[0023] Therefore, camera observation Modeling as
[0024]
[0025] in, Indicates the number of feature points. Represents the camera observation function. Represents the Gaussian distribution. Indicates camera observation noise. The covariance matrix represents the observation noise.
[0026] Step 13: Construct a GNSS / INS observation model;
[0027] consider Current GNSS observations of the system's position and INS observations of the system's orientation The following GNSS / INS observation model is constructed.
[0028]
[0029] in, Represents the observation function of GNSS / INS. Represents a logarithmic mapping. This represents observation noise from GNSS / INS. The covariance matrix represents the observation noise.
[0030] Step 14: Construct the overall observation model;
[0031] Through tightly coupled camera, GNSS / INS sensor, and observation model Build as follows
[0032]
[0033] in, , The Jacobian matrix of the observation model for the error state is expressed as follows: The linearization point is chosen as the nominal state at the current time step.
[0034] Compared to traditional system state modeling methods, this invention establishes the robot's orientation state within a manifold space, effectively representing the three-degree-of-freedom rotational state. Compared to a single sensor, this invention tightly couples observations from three sensors—camera, IMU, and GNSS / INS—and constructs an observation model, effectively compensating for the shortcomings of a single sensor and improving the stability of the positioning system in complex environments.
[0035] Preferably, step 2 specifically includes:
[0036] Step 21, Model GNSS / INS noise observations;
[0037] Considering that in real-world scenarios, GNSS / INS observations are typically affected by multipath propagation, cloud cover, or transient impulsive noise within the sensor, GNSS / INS measurements are often accompanied by non-static heavy-tailed noise. Therefore, the observation noise from GNSS / INS can be modeled as a GSTM distribution, i.e., the observation likelihood. Modeled as a GSTM distribution, where Represents the mixture probability. Given the mixture probability... The expression for the observation likelihood is as follows:
[0038]
[0039] in, , This represents Student's t-distribution. Represents the Gamma distribution. This represents the degree of freedom parameter. It is achieved by introducing Bernoulli random variables. GSTM can be decomposed into a hierarchical Gaussian form. The probability mass function is as follows:
[0040]
[0041] Among them, parameters The prior probability density function is a Beta distribution, i.e.
[0042]
[0043] in, These are the prior shape parameters. The layered Gaussian form is as follows:
[0044]
[0045] in, The prior distribution is a Gamma distribution, expressed as:
[0046]
[0047] Step 22, System measurement acquisition;
[0048] The system's control inputs come from the acceleration and angular velocity observations of the INS, and the observations come from the orientation observations of the INS, the position observations of the GNSS, and the observations of feature points extracted from the images by the binocular camera;
[0049] Step 23: Solve for the approximate posterior probability of the latent variables;
[0050] Define a set of implicit variables The posterior probability distribution of all elements in the latent variable set is inferred using variational inference. Since real-world probability models are very complex, it is difficult to directly obtain the exact posterior probability distribution. Therefore, based on mean-field theory, the following probability distribution can be used to approximate the posterior.
[0051]
[0052] According to the theory of variational inference, This can be obtained by constructing the following optimization problem.
[0053]
[0054] in, Denotes the divergence. The solution to this optimization problem satisfies the following conclusions.
[0055]
[0056] in express Any element in, express The supplement, Then it represents a constant. Denote it as a superscript. Indicates the first The second fixed-point iteration, in time step Below, for parameters The The result of the second iteration needs to be used. The result of the remaining parameters in the next iteration According to Bayesian theory, observations are relative to the state vector. They are conditionally independent, therefore the joint probability density function It can be factored into the following form
[0057]
[0058] Due to the coupling of variational parameters, a fixed-point iterative approach is needed to find an approximate optimal solution. Let the latent variables... , It is updated to a Gaussian distribution, expressed as follows:
[0059]
[0060] in, , They represent The posterior estimates of the mean and covariance matrix. According to the relevant theory of ESKF, The posterior probability distribution needs to be calculated in two steps. The first step is the prediction step, i.e.
[0061]
[0062] in, It follows a Gaussian distribution, with a mean and covariance of respectively. , The second step is the observation update step. The system error state update equation is as follows:
[0063]
[0064] in, , The corrected noise covariance matrix is expressed as follows:
[0065]
[0066] Combined with pullback function You can get The posterior update state, where This represents the addition operation between composite manifolds.
[0067] Let hidden variables , The approximate posterior distribution of the probability mass function can be expressed as the Bernoulli distribution, and its probability mass function can be expressed as...
[0068]
[0069] in, This represents the normalization constant. and The expression is as follows
[0070]
[0071] Let hidden variables , The approximate posterior distribution of can be expressed as a Gamma distribution, and its probability density function can be expressed as .
[0072]
[0073] Among them, parameters , The expression is as follows
[0074]
[0075] Let hidden variables , The approximate posterior distribution of the probability density function can be expressed as a Beta distribution, and its probability density function can be expressed as...
[0076]
[0077] Among them, parameters , The expression is as follows
[0078]
[0079] The expected value appearing in the above approximate posterior is calculated by the following equation:
[0080]
[0081] in, This represents the Digamma function.
[0082] Define the maximum number of iterations at a fixed point as And set the termination condition as
[0083]
[0084] in, This represents subtraction in a combinatorial manifold space. These are the set algorithm parameters. The fixed-point iteration terminates when the above conditions are met. After termination or reaching the maximum number of iterations, let... , .
[0085] Step 24: Determine if the condition for stopping iteration has been met. If not, return to step 22.
[0086] This invention employs a Gaussian-Student's t mixture distribution to model observation noise, effectively capturing the time-varying and non-Gaussian characteristics of observation noise in complex environments. Subsequently, variational inference techniques are used to jointly estimate the system state and noise parameters, effectively improving the robustness and accuracy of the robot localization system.
[0087] Based on the same inventive concept, the present invention provides a multi-source heterogeneous robust navigation and positioning system based on error state variational inference, comprising:
[0088] The model building module first constructs a state vector model of the system based on the actual operation of the system; then, based on the acceleration and angular velocity observations of the INS, it constructs an error state propagation model of the system; finally, based on the INS, GNSS, and camera, it constructs a state observation model of the system.
[0089] Furthermore, the robust estimation module first considers the heavy-tailed characteristics of actual GNSS / INS noise and models the noise of the integrated navigation as a Gaussian-Student's t mixture distribution. Subsequently, combined with ESKF, it jointly infers the posterior probability distribution of approximate latent variables based on variational inference techniques, and sets an iteration termination condition. When the iteration termination condition is reached, the fixed-point iteration is completed, and the estimation results are given.
[0090] Based on the same inventive concept, the present invention provides a computer system, including a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, when the computer program is loaded onto the processor, it implements the steps of the multi-source heterogeneous robust navigation and positioning system based on error state variational inference as described in any one of claims 1-4.
[0091] Beneficial Effects: The multi-source heterogeneous robust navigation and positioning system based on error state variational inference proposed in this invention first constructs a system state vector according to the actual operating scenario of the system and uses INS to construct a system state propagation model; then, it establishes a sensor observation model using GNSS position observations, INS angle observations, and camera observations of feature points; next, it models the GNSS / INS position observation noise and angle observation noise as GSTM distributions and constructs latent variables of noise parameters based on these distributions; finally, it uses variational inference and ESKF to jointly estimate the probability distribution of system state and noise. The system state estimation model established in this invention, along with its iterative method for approximate posterior distribution of latent variables, can improve the robustness and estimation accuracy of the algorithm when outliers exist in GNSS / INS observation noise; simulation results and experimental data show that this invention can achieve accurate system estimation. Attached Figure Description
[0092] Figure 1 This is a general flowchart of the method according to an embodiment of the present invention.
[0093] Figure 2 This is a flowchart of the variational inference joint estimation system state in an embodiment of the present invention.
[0094] Figure 3 This is a graph showing the estimated system state position error in this embodiment.
[0095] Figure 4 This is a diagram showing the system state orientation error curve estimated in this embodiment.
[0096] Figure 5 The system state velocity error curve is estimated for this embodiment. Detailed Implementation
[0097] 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, and 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.
[0098] Depend on Figure 1 As shown in the embodiments of the present invention, the multi-source heterogeneous robust navigation and positioning system based on error state variational inference first constructs the system state by comprehensively considering the system's operating scenarios and requirements. Then, based on the acceleration and angular velocity observations of the shipborne INS, a system state propagation model is constructed. Subsequently, considering the INS' angle observations, the camera sensor's feature point observations, and the GNSS's position observations, a system state observation model is constructed. Finally, considering that the observation noise of the integrated navigation GNSS / INS has outliers, the observation noise is modeled as a Gaussian-Student's t-distribution, and a joint estimation scheme for the latent variables of the system state and noise parameters based on variational inference and ESKF is designed to estimate the system state and noise parameters in real time.
[0099] The following uses the system state estimation of an unmanned surface vessel as an example to illustrate the detailed implementation process of this invention, including the following specific steps:
[0100] S1, Construct the state, state propagation model and state observation model of the unmanned vessel system;
[0101] S11, Model the state of the unmanned vessel system and construct a state propagation model;
[0102] Based on actual operating scenarios of unmanned surface vessels (USVs), the real states of USVs at discrete time points are constructed. nominal state and error state for
[0103]
[0104]
[0105]
[0106] in, , , These represent the system in the world coordinate system. The actual state, nominal state, and error state of the next position. , , Indicates the system in the world coordinate system The true state, nominal state, and error state of velocity at any given time. , , This represents the true, nominal, and error states of the system's orientation in the world coordinate system. , , This indicates the true state, nominal state, and error state of the acceleration zero bias of the system's INS. , , This indicates the true state, nominal state, and error state of the angular velocity zero bias of the system's INS. , , This represents the true, nominal, and error states of gravitational acceleration. The error state is determined by integrating the differential equation and combining it with acceleration observations from the INS. and angular velocity observation The propagation model of the systematic error state can be described as follows:
[0107]
[0108] in This indicates process noise disturbance. and These represent the state transition functions. For the Jacobian matrix of the error state and the perturbation, and Let these represent the predicted nominal state at the current time step and the updated nominal state at the previous time step, respectively. Indicates IMU observations;
[0109] S12, Construct the camera observation model;
[0110] consider At this moment The observation of each feature point in the camera coordinate system is as follows:
[0111]
[0112] Feature point observation is established in the camera coordinate system. Down, , , These represent the coordinates in the three directions, respectively. Based on the system estimation results, the estimated values of the feature points are calculated as follows:
[0113]
[0114] in, This represents the feature point estimation results. and These represent the rotation and translation from the INS coordinate system to the camera coordinate system, respectively. The estimated value of the feature point in the world coordinate system can be approximated by deploying the least squares method. Based on the observed and estimated values of the feature point, the following observation residuals are constructed.
[0115]
[0116] Therefore, camera observation Modeling as
[0117]
[0118] in, Indicates the number of feature points. Represents the camera observation function. Represents the Gaussian distribution. Indicates camera observation noise. The covariance matrix represents the observation noise.
[0119] S13, Construct GNSS / INS observation model;
[0120] consider Real-time GNSS observations of the unmanned surface vessel's position and INS observations of the system's orientation The following GNSS / INS observation model is constructed.
[0121]
[0122] in, Represents the observation function of GNSS / INS. Represents a logarithmic mapping. This represents observation noise from GNSS / INS. The covariance matrix represents the observation noise.
[0123] S14, Construct the overall observation model;
[0124] The observation model is constructed by tightly coupling a shipborne camera and a GNSS / INS sensor as follows:
[0125]
[0126] in, , The Jacobian matrix of the observation model for the error state is expressed as follows: The linearization point is chosen as the nominal state at the current time step.
[0127] S2, modeling abnormal GNSS / INS observation noise, using variational inference combined with ESKF to jointly infer the approximate posterior distribution of latent variables, to achieve unmanned vessel state estimation;
[0128] S21, Modeling GNSS / INS noise observations;
[0129] Considering that in real-world scenarios, GNSS / INS observations are typically affected by multipath propagation, cloud cover, or transient impulsive noise within the sensor, GNSS / INS measurements are often accompanied by non-static heavy-tailed noise. Therefore, the observation noise from GNSS / INS can be modeled as a GSTM distribution, i.e., the observation likelihood. Modeled as a GSTM distribution, where Represents the mixture probability. Given the mixture probability... The expression for the observation likelihood is as follows:
[0130]
[0131] in, , This represents Student's t-distribution. Represents the Gamma distribution. This represents the degree of freedom parameter. It is achieved by introducing Bernoulli random variables. GSTM can be decomposed into a hierarchical Gaussian form. The probability mass function is as follows:
[0132]
[0133] Among them, parameters The prior probability density function is a Beta distribution, i.e.
[0134]
[0135] in, These are the prior shape parameters. The layered Gaussian form is as follows:
[0136]
[0137] in, The prior distribution is a Gamma distribution, expressed as:
[0138]
[0139] S22, Measurement and acquisition by unmanned surface vessel systems;
[0140] The control inputs of the unmanned surface vessel system come from the acceleration and angular velocity observations of the INS, and the observations come from the orientation observations of the INS, the position observations of the GNSS, and the observations of feature points extracted from images by the binocular camera.
[0141] S23, Solving for approximate posterior probabilities of latent variables;
[0142] Define a set of implicit variables The posterior probability distribution of all elements in the latent variable set is inferred using variational inference. Since real-world probability models are very complex, it is difficult to directly obtain the exact posterior probability distribution. Therefore, based on mean-field theory, the following probability distribution can be used to approximate the posterior.
[0143]
[0144] According to the theory of variational inference, This can be obtained by constructing the following optimization problem.
[0145]
[0146] in, Denotes the divergence. The solution to this optimization problem satisfies the following conclusions.
[0147]
[0148] in express Any element in, express The supplement, Then it represents a constant. Denote it as a superscript. Indicates the first The second fixed-point iteration, in time step Below, for parameters The The result of the second iteration needs to be used. The result of the remaining parameters in the next iteration According to Bayesian theory, observations are relative to the state vector. They are conditionally independent, therefore the joint probability density function It can be factored into the following form
[0149]
[0150] Due to the coupling of variational parameters, a fixed-point iterative approach is needed to find an approximate optimal solution. Let the latent variables... , It is updated to a Gaussian distribution, expressed as follows:
[0151]
[0152] in, , They represent The posterior estimates of the mean and covariance matrix. According to the relevant theory of ESKF, The posterior probability distribution needs to be calculated in two steps. The first step is the prediction step, i.e.
[0153]
[0154] in, It follows a Gaussian distribution, with a mean and covariance of respectively. , The second step is the observation update step. The system error state update equation is as follows:
[0155]
[0156] in, , The corrected noise covariance matrix is expressed as follows:
[0157]
[0158] Combined with pullback function You can get The posterior update state, where This represents the addition operation between composite manifolds.
[0159] Let hidden variables , The approximate posterior distribution of the probability mass function can be expressed as the Bernoulli distribution, and its probability mass function can be expressed as...
[0160]
[0161] in, This represents the normalization constant. and The expression is as follows
[0162]
[0163] Let hidden variables , The approximate posterior distribution of can be expressed as a Gamma distribution, and its probability density function can be expressed as .
[0164]
[0165] Among them, parameters , The expression is as follows
[0166]
[0167] Let hidden variables , The approximate posterior distribution of the probability density function can be expressed as a Beta distribution, and its probability density function can be expressed as...
[0168]
[0169] Among them, parameters , The expression is as follows
[0170]
[0171] The expected value appearing in the above approximate posterior is calculated by the following equation:
[0172]
[0173] in, This represents the Digamma function.
[0174] Define the maximum number of iterations at a fixed point as And set the termination condition as
[0175]
[0176] in, This represents subtraction in a combinatorial manifold space. These are the set algorithm parameters. The fixed-point iteration terminates when the above conditions are met. After termination or reaching the maximum number of iterations, let... , .
[0177] S24, determine whether the condition for stopping iteration has been met; if not, return to step 22.
[0178] This invention employs a Gaussian-Student's t mixture distribution to model observation noise, effectively capturing the time-varying and non-Gaussian characteristics of observation noise in complex environments. Subsequently, variational inference techniques are used to jointly estimate the system state and noise parameters, effectively improving the robustness and accuracy of the robot localization system.
[0179] In this embodiment, MATLAB 2023a is used as the simulation software to compare the robust ESKF of the present invention, which considers GNSS / INS observation noise, with the traditional ESKF method.
[0180] The simulation parameters are set as follows: sampling period The Monte Carlo experiment was conducted 50 times, with a duration of 200 seconds. The GNSS / INS measurement noise was set to...
[0181]
[0182] in, Indicates probability, , , The INS acceleration and angular velocity observations are set to...
[0183]
[0184] For robust ESKF and the ESKF algorithm, the initial value of the state is set to ,in The covariance matrix of GNSS / INS measurement noise is set to Additionally, for robust ESKF, setting parameters... , , , .
[0185] Figure 3 , Figure 4 and Figure 5 These represent parameter errors respectively. , and The iteration curve, , and Defined respectively
[0186]
[0187] in, , and Let represent the true values of the unmanned vessel's position, orientation, and velocity at time s, respectively. , and Let represent the estimated positions, orientations, and velocities of the unmanned surface vessel at time k, respectively. This indicates that the simulation of the Monte Carlo experiment was repeated 50 times.
[0188] Figure 3 , Figure 4 and Figure 5 The solid line labeled ESKF represents the error in the position and orientation of the unmanned surface vessel system estimated by the traditional ESKF method, while the dashed line labeled VB-ESKF represents the error of the multi-source heterogeneous robust navigation and positioning system based on error state variational inference proposed in this invention. It can be seen that the results obtained by considering GNSS / INS noise and performing variational inference in this invention provide better estimation results. Furthermore, Table 1 provides specific details... , and Compared to traditional algorithms, the algorithm designed in this invention can effectively reduce the ARMSE value and improve the accuracy of robot state estimation. , and The calculation formula is as follows:
[0189]
[0190]
[0191] Table 1 ARMSE Calculation Results
[0192] Based on the same inventive concept, the multi-source heterogeneous robust navigation and positioning system based on error state variational inference disclosed in this invention includes: a model building module, which first constructs a state vector model of the unmanned vessel system based on the actual operation of the unmanned vessel; then, constructs an error state propagation model of the unmanned vessel based on acceleration and angular velocity observations from the shipborne INS; finally, constructs a state observation model of the unmanned vessel based on the shipborne INS, GNSS, and camera; and a robust estimation module, which first considers the heavy-tailed characteristics of the noise of the actual GNSS / INS and models the noise of the combined navigation as a Gaussian-Student's t mixture distribution; then, combined with ESKF, it jointly infers the posterior probability distribution of the approximate latent variables based on variational inference technology, and sets an iteration termination condition. When the iteration termination condition is reached, the fixed-point iteration is completed, and the estimation result is given.
[0193] The specific working processes of each module described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here. The division of modules is only a logical functional division, and there may be other division methods in actual implementation. For example, multiple modules may be combined or integrated into another system.
[0194] Based on the same inventive concept, an embodiment of the present invention discloses a computer system including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the computer program is loaded onto the processor, it implements the steps of the multi-source heterogeneous robust navigation and positioning system based on error state variational inference.
[0195] Those skilled in the art will understand that the technical solution of this invention, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer system (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the method described in the embodiments of this invention. The storage medium includes various media capable of storing computer programs, such as a USB flash drive, portable hard drive, read-only memory (ROM), random access memory (RAM), magnetic disk, or optical disk.
[0196] The above are merely preferred embodiments of the present invention and are not intended to limit the scope of 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 multi-source heterogeneous robust navigation and positioning method based on error state variational inference, characterized in that, Includes the following steps: Step 1: Construct a system state vector based on the actual operating scenario of the system, and use ESKF to predict and update the system state based on sensor observations from INS, GNSS and cameras. Specifically, this includes using INS acceleration and angular velocity measurements to construct a system error state propagation model, and then establishing a sensor observation model based on GNSS position observations, camera feature point observations and INS angle observations. Step 2: Robust real-time estimation of system state and noise parameters based on variational inference. Specifically, this includes modeling GNSS / INS position observation noise and angle observation noise as GSTM distributions, constructing latent variables of noise parameters based on these distributions, and using variational inference and ESKF to jointly estimate the probability distributions of system state and noise.
2. The multi-source heterogeneous robust navigation and positioning method based on error state variational inference according to claim 1, characterized in that, Step 1 specifically includes: Step 11: Model the system state and construct the state propagation model; Based on the actual operation scenario of the system, construct the real state of the system at discrete time points. nominal state and error state for in, , , These represent the system in the world coordinate system. The actual state, nominal state, and error state of the next position. , , Indicates the system in the world coordinate system The true state, nominal state, and error state of velocity at any given time. , , This represents the true, nominal, and error states of the system's orientation in the world coordinate system. , , This indicates the true state, nominal state, and error state of the acceleration zero bias of the system's INS. , , This indicates the true state, nominal state, and error state of the angular velocity zero bias of the system's INS. , , This represents the true state, nominal state, and error state of gravitational acceleration. The error state is determined by integrating the differential equation and combining it with acceleration observations from the INS. and angular velocity observation The propagation model of the system error state is described as follows: in This indicates process noise disturbance. and These represent the state transition functions. For the Jacobian matrix of the error state and the perturbation, and Let these represent the predicted nominal state at the current time step and the updated nominal state at the previous time step, respectively. Indicates IMU observations; Step 12, construct the camera observation model; consider At this moment The observation of each feature point in the camera coordinate system is as follows: Feature point observation is established in the camera coordinate system. Down, , , Let represent the coordinates in the three directions respectively. Based on the system estimation results, the estimated values of the feature points are calculated as follows: in, This represents the feature point estimation results. and These represent the rotation and translation from the INS coordinate system to the camera coordinate system, respectively. The estimated value of the feature point in the world coordinate system is obtained by deploying the least squares method. Based on the observed and estimated values of the feature point, the following observation residual is constructed. Therefore, camera observation Modeling as in, Indicates the number of feature points. Represents the camera observation function. Represents the Gaussian distribution. Indicates camera observation noise. The covariance matrix representing the observation noise. Step 13: Construct a GNSS / INS observation model; consider Current GNSS observations of the system's position and INS observations of the system's orientation The following GNSS / INS observation model is constructed. in, Represents the observation function of GNSS / INS. Represents a logarithmic mapping. This represents observation noise from GNSS / INS. The covariance matrix representing the observation noise. Step 14: Construct the overall observation model; Through tightly coupled camera, GNSS / INS sensor, and observation model Build as follows in, , Representing observation noise, the Jacobian matrix of the observation model with respect to the error state is expressed as follows: The linearization point is chosen as the nominal state at the current time step.
3. The multi-source heterogeneous robust navigation and positioning method based on error state variational inference according to claim 1, characterized in that, Step 2 specifically includes: Step 21, Model GNSS / INS noise observations; The observation noise from GNSS / INS is modeled as a GSTM distribution, i.e., the observation likelihood. Modeled as a GSTM distribution, where Represents the mixture probability, given the mixture probability The expression for the observation likelihood is as follows: in, , This represents Student's t-distribution. Represents the Gamma distribution. The degree of freedom parameter is represented by introducing Bernoulli random variables. GSTM is decomposed into a hierarchical Gaussian form. The probability mass function is as follows: Among them, parameters The prior probability density function is a Beta distribution, i.e. in, Given the prior shape parameters, the layered Gaussian form is as follows: in, The prior distribution is a Gamma distribution, expressed as: Step 22, System measurement acquisition; The system's control inputs come from the acceleration and angular velocity observations of the INS, and the observations come from the orientation observations of the INS, the position observations of the GNSS, and the observations of feature points extracted from the images by the binocular camera; Step 23: Solve for the approximate posterior probability of the latent variables; Define a set of implicit variables Furthermore, variational inference is used to infer the posterior probability distribution of all elements in the set of latent variables. Based on mean-field theory, the following probability distribution is used to approximate the posterior. According to the theory of variational inference, By constructing the following optimization problem, we obtain... in, Let the divergence be denoted by the following conclusion: in express Any element in, express The supplement, Then represents a constant, denoted by a superscript. Indicates the first The second fixed-point iteration, in time step Below, for parameters The The result of the second iteration needs to be used. The result of the remaining parameters in the next iteration According to Bayesian theory, observations are relative to the state vector. They are conditionally independent, therefore the joint probability density function Factorize into the following form Due to the coupling of variational parameters, it is necessary to use fixed-point iteration to solve for an approximate optimal solution, letting the latent variables... , It is updated to a Gaussian distribution, expressed as follows: in, , They represent The posterior estimates of the mean and covariance matrix, based on the relevant theories of ESKF, The posterior probability distribution needs to be calculated in two steps. The first step is the prediction step, i.e. in, It follows a Gaussian distribution, with a mean and covariance of respectively. , The second step is the observation update step, and the system error state update equation is as follows: in, , The corrected noise covariance matrix is expressed as follows: Combined with pullback function ,get The posterior update state, where This represents the addition operation between composite manifolds. Let hidden variables , The approximate posterior distribution of is expressed as the Bernoulli distribution, and its probability mass function is expressed as . in, Represents the normalization constant. and The expression is as follows Let hidden variables , The approximate posterior distribution of is expressed as the Gamma distribution, and its probability density function is expressed as . Among them, parameters , The expression is as follows Let hidden variables , The approximate posterior distribution of is expressed as a Beta distribution, and its probability density function is expressed as . Among them, parameters , The expression is as follows The expected value appearing in the above approximate posterior is calculated by the following equation: in, Represents the Digamma function. Define the maximum number of iterations at a fixed point as And set the termination condition as in, This represents subtraction in a combinatorial manifold space. When the set algorithm parameters satisfy the above conditions, the fixed-point iteration terminates. After termination or reaching the maximum number of iterations, let... , , Step 24: Determine if the condition for stopping iteration has been met. If not, return to step 22.
4. A multi-source heterogeneous robust navigation and positioning system based on error state variational inference, characterized in that, include: The model building module first constructs a state vector model of the system based on the actual operation of the system; then, based on the acceleration and angular velocity observations of the INS, it constructs an error state propagation model of the system; finally, based on the INS, GNSS, and camera, it constructs a state observation model of the system. In addition, the robust estimation module first considers the heavy-tailed characteristics of actual GNSS / INS noise and models the noise of the integrated navigation as a Gaussian-Student's t mixture distribution. Subsequently, by combining ESKF and using variational inference techniques, the posterior probability distribution of the approximate latent variables is jointly inferred. An iteration termination condition is set, and when the iteration termination condition is met, the fixed-point iteration is completed, and the estimation results are given.
5. A computer system comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the computer program is loaded into the processor, it implements the steps of the multi-source heterogeneous robust navigation and positioning system based on error state variational inference according to any one of claims 1-4.