Inertia and acoustics integrated navigation method based on enhanced hybrid minimum error entropy and unscented Kalman filtering

Abstract

The invention provides an inertia and acoustics integrated navigation method based on enhanced hybrid minimum error entropy and unscented Kalman filtering. The method comprises the following steps: establishing a state equation model of an inertia and acoustics integrated navigation system; establishing an enhanced mixed minimum error entropy measurement model considering a sound ray bending effect and a multi-modal noise characteristic; based on a state equation model and an enhanced hybrid minimum error entropy measurement model, an EnMMEE-UKF framework is constructed, nonlinear mapping is processed through unscented transformation, an attenuation factor in a strong tracking filtering theory is introduced to correct an error covariance matrix, and an expectation maximization algorithm is adopted to adaptively adjust a hybrid coefficient of a hybrid kernel function. And state estimation and a covariance matrix are recursively updated in combination with a fixed point iteration method, so that real-time updating and feedback of errors are realized.

Description

Technical Field

[0001] This application relates to the field of underwater positioning technology, and in particular to an inertial and acoustic integrated navigation method based on enhanced hybrid minimum error entropy and unscented Kalman filtering (EnMMEE-UKF). Background Technology

[0002] Autonomous underwater vehicles (AUVs) originated from the need for autonomous underwater exploration and are primarily used for unmanned, long-term underwater missions, such as environmental monitoring and seabed resource exploration. Because electromagnetic signals are prone to severe attenuation in water, satellite navigation methods are not suitable for underwater navigation; they are only suitable for use when the vehicle is moving on the water surface. Therefore, achieving high-precision, high-performance underwater navigation technology has always been a bottleneck problem in the development and application of underwater robots.

[0003] Inertial navigation systems (INS) do not rely on external signals. They measure changes in motion solely through internal accelerometers and gyroscopes and are widely used in underwater navigation. However, a drawback of INS is that errors accumulate over long-term use, requiring correction through other methods. Due to the low attenuation of underwater acoustic signals, acoustic positioning methods are an alternative for underwater navigation and positioning.

[0004] Integrated navigation systems refer to navigation technologies that optimally integrate measurement information from two or more navigation systems through information fusion algorithms to achieve complementary advantages. The core of this technology lies in comprehensively utilizing the characteristics of different navigation sensors and improving the overall accuracy and reliability of navigation performance through data fusion processing. Inertial and acoustic integrated navigation is currently a relatively mature and widely used underwater integrated navigation method. However, in actual underwater operations, the curvature of sound rays and the complex and harsh underwater environment can lead to erroneous data, abnormal data, and data loss from acoustic sensors, resulting in decreased navigation accuracy. A search revealed that Chinese invention patent CN119714255A discloses an inertial and acoustic integrated navigation method considering the curvature of sound rays. The method includes: Step 1, establishing a state equation model for inertial and acoustic integrated navigation; Step 2, establishing a curved sound ray measurement model based on an equal gradient sound velocity profile; Step 3, updating and feeding back errors using an unscented Kalman filter method based on the state equation and measurement equation established in Steps 1 and 2.

[0005] The aforementioned prior art differs from this application in the following ways:

[0006] 1. The prior art focuses on the problem of acoustic positioning in deep sea affected by the curvature of sound rays, leading to positioning errors in integrated navigation. It pertains to precise model building and data processing techniques. The core objective is to establish a complex mathematical model based on a curved sound ray measurement model with an equal gradient sound velocity profile, and to use an unscented Kalman filter algorithm to handle the strong nonlinearity introduced by this model, ultimately improving the accuracy of integrated positioning. This application focuses on solving the problem of decreased accuracy in inertial / acoustic integrated navigation caused by complex underwater environments (curved sound rays, multimodal noise, measurement anomalies), belonging to underwater navigation and information fusion technologies. The core objective is to construct a highly robust and computationally inefficient filtering algorithm by fusing the enhanced hybrid minimum error entropy criterion with unscented Kalman filtering, achieving high-precision and reliable state estimation of inertial / acoustic integrated navigation systems in complex underwater environments. 2. The prior art addresses the model error problem in underwater integrated navigation systems caused by curved sound rays. By deriving a new measurement model and combining it with unscented Kalman filtering to solve the model nonlinearity problem, it improves the positioning accuracy of inertial / acoustic integrated navigation under curved sound ray conditions. This application addresses the problems of inaccurate estimation and poor robustness in underwater integrated navigation caused by complex non-Gaussian noise, acoustic ray bending, and measurement anomalies. By fusing and enhancing the hybrid minimum error entropy criterion with unscented Kalman filtering, it achieves improved state estimation and robustness of the integrated navigation system in complex underwater environments. 3. The prior art is used for deep-sea underwater integrated navigation, with users being researchers, focusing on correcting system model errors. This application is used for underwater integrated navigation in harsh underwater acoustic environments with strong interference, with users being researchers, focusing on the robustness of the filtering algorithm under non-Gaussian noise and outliers. Although both patents relate to underwater integrated navigation, Patent 1 belongs to the field of accurate model building, solving the problem of correcting system model errors; Patent 2 belongs to the field of information fusion, solving the problems of inaccurate estimation and poor robustness in integrated navigation. The technical solutions, application scenarios, and effects do not overlap, and they are complementary technologies. Summary of the Invention

[0007] Purpose of the invention: This application provides an inertial and acoustic integrated navigation method based on enhanced hybrid minimum error entropy and unscented Kalman filtering (EnMMEE-UKF), which can be used to solve the problems of ray bending, multimodal noise and measurement data anomalies in underwater inertial / acoustic integrated navigation systems.

[0008] Technical Solution: This invention provides an inertial and acoustic integrated navigation method based on enhanced hybrid minimum error entropy and unscented Kalman filtering, the method comprising the following steps:

[0009] Step (1): Establish the state equation model of the inertial and acoustic integrated navigation system;

[0010] Step (2): Establish an enhanced hybrid minimum error entropy measurement model that considers the sound ray bending effect and multimodal noise characteristics. The model is based on the construction of a curved sound ray propagation model on an equal gradient sound velocity profile, and introduces a hybrid kernel function and quantization operator to optimize the error entropy calculation in order to adapt to the complex underwater measurement environment.

[0011] Step (3): Based on the state equation model in step (1) and the enhanced hybrid minimum error entropy measurement model in step (2), the EnMMEE-UKF framework is constructed. Nonlinear mapping is processed by unscented transformation (UT). The attenuation factor in strong tracking filter (STF) theory is introduced to correct the error covariance matrix. The expectation-maximization (EM) algorithm is used to adaptively adjust the mixing coefficients of the hybrid kernel function. The state estimate and covariance matrix are recursively updated by combining the fixed-point iterative method to realize the real-time update and feedback of the error.

[0012] Furthermore, the state equation model for the inertial and acoustic integrated navigation system established in step (1) is as follows:

[0013] Define the system state vector The state vector includes critical errors in inertial navigation and inherent errors in the sensors:

[0014] ;

[0015] in, This is the carrier attitude misalignment angle error vector, reflecting the deviation between the attitude calculation of the inertial navigation system and the actual attitude; This is the vehicle velocity error vector, describing the difference between the calculated velocity and the actual velocity in inertial navigation. This is the carrier position error vector. This indicates the zero bias of the gyroscope. This indicates the zero bias of the accelerometer;

[0016] Therefore, the state equation for integrated navigation is:

[0017] ;

[0018] in, Represents the state transition matrix. The process noise vector consists of gyroscope random walk, accelerometer random walk, and zero-biased random fluctuation terms, following a Gaussian or Gaussian mixture distribution.

[0019] Furthermore, in step (2), an enhanced hybrid minimum error entropy measurement model considering the ray bending effect and multimodal noise characteristics is established, specifically as follows:

[0020] Approximating the underwater sound field as a uniform gradient distribution, the relationship between sound velocity and depth is as follows:

[0021] ;

[0022] in, Indicates depth The speed of sound at the surface of the water is given by g, which represents the speed gradient of sound, and b, which is the speed of sound at the surface of the water.

[0023] Based on Snell's criterion, the propagation of sound rays satisfies the condition that "the ratio of the speed of sound to the cosine of the angle between the sound velocity and the perpendicular to the direction of propagation is constant," expressed as:

[0024] ;

[0025] in, For depth The angle between the sound ray at that point and the direction of the tangent at that point. The angle of incidence of the sound ray at the transponder. The exit angle of the sound ray at the acoustic transducer; and Indicates the depth of the transponder or acoustic transducer;

[0026] Based on ray tracing theory and combined with constant gradient sound velocity profiles, an analytical expression for the propagation time t of sound waves under curved sound lines is derived:

[0027] ;

[0028] in, , , , , This indicates the horizontal distance between the transponder and the acoustic transducer.

[0029] The acoustic system obtains the azimuth angle of the transponder through a transducer array. and Its three-dimensional coordinates relative to the transponder in the transponder coordinate system The relationship is:

[0030] ;

[0031] Based on the Raney entropy theory, the error of The information potential energy of order is defined as:

[0032] ;

[0033] in, For error The probability density function (pdf). For expectation operator, Entropy order, information potential energy It is negatively correlated with error entropy, and maximizes This is equivalent to minimizing the error entropy, which enhances robustness to non-Gaussian noise.

[0034] To adapt to multimodal noise distribution, two Gaussian kernel functions are fused as a mixture kernel for estimating the error pdf, expressed as:

[0035] ;

[0036] in, , The mixing coefficient, , ; , These are two single-modal Parzen window PDF estimators; For Gaussian kernel function, , The kernel width is determined based on the standard deviation of the error samples; The number of error samples. For the first Error samples, .

[0037] Introducing quantization operators reduces the computational complexity of the hybrid minimum error entropy. Quantization operator definition: A scalar quantization operator is used. ,Will Error samples ( )) Quantified to Codebook ,in For the first Each codebook value This is for quantization step size.

[0038] Based on codebook and quantized sample counting, the information potential energy of the hybrid minimum error entropy is simplified to:

[0039] ;

[0040] in, To quantify to the first The number of error samples in each codebook satisfies After quantization, the computational complexity decreased from... Reduce to ,because The computational efficiency is significantly improved.

[0041] By introducing strong tracking characteristics and statistical linearization to optimize robustness, the nonlinear function in the measurement equation is approximated as a linear relationship through statistical linear regression, and the measurement slope matrix is ​​defined as follows:

[0042] ;

[0043] in, For state one-step prediction error The covariance matrix, for The cross-covariance matrix with measurement error.

[0044] By statistical linearization, the nonlinear measurement equation Approximately:

[0045] ;

[0046] in, The measurement noise vector follows a multimodal distribution, and its covariance matrix is... .

[0047] Based on the strong tracking filter (STF) theory, an attenuation factor is introduced. Correcting the state one-step prediction error covariance matrix To enhance the ability to track dynamic disturbances, The calculation method is as follows:

[0048] ;

[0049] in, The actual covariance matrix of the measurement residuals. Here is the theoretical covariance matrix of the measurement residuals; the corrected state one-step prediction error covariance matrix is... .

[0050] The enhanced hybrid minimum error entropy criterion is used as the filtering cost function to replace the traditional UKF mean square error (MSE) cost function. The expression is as follows:

[0051] ;

[0052] in, This is a regularization matrix used to prevent the estimated value from being too large and to constrain the cost function.

[0053] Furthermore, in step 3, the EnMMEE-UKF framework is constructed to achieve state estimation and error feedback, specifically as follows:

[0054] Step 1: Initialize the filter parameters and state variables to provide initial conditions for the filter iteration; initialize the hybrid kernel function coefficients. Set the convergence threshold for the EM algorithm iteration. With maximum number of iterations .

[0055] Step 2: Generate sigma points using unscented transformation to approximate the probability distribution of the state. The specific steps are as follows:

[0056] Step 2a, calculate The square root matrix of the state estimation at time step: Performing Cholesky decomposition yields... ,satisfy .

[0057] Step 2b, generate sigma points:

[0058] ;

[0059] ;

[0060] ;

[0061] in, express The List.

[0062] Step 3, execute the time update step, and predict. The state and covariance at time t, specifically including:

[0063] Step 3a, one-step prediction of sigma points: Substituting the sigma point at time t into the discretized state equation, we get:

[0064] ;

[0065] in, The process noise sigma point is determined by... The generation follows the same UT transformation rules as the state sigma point.

[0066] Step 3b, Prediction of the prior state mean: The value is predicted in one step by weighted summation of sigma points, yielding:

[0067]

[0068] in, The mean weight is used to ensure that the predicted mean is unbiased.

[0069] Step 3c, Predicting the prior error covariance:

[0070] ;

[0071] in, For covariance weights, This represents the process noise covariance matrix.

[0072] Step 3d, Attenuation factor correction: Calculate the attenuation factor at time k based on strong tracking theory. And correct the prior covariance:

[0073] ;

[0074] like Then take ,make sure Zhengding.

[0075] Step 4, execute the measurement update step, combining the EnMMEE criterion and the EM algorithm to optimize the state estimation, specifically including:

[0076] Step 4a, Generate measurement sigma points: for the corrected prior covariance Perform UT transformation to generate measurement sigma points:

[0077] ;

[0078] ;

[0079] in, .

[0080] Substituting the measured sigma point into the nonlinear measurement function, we obtain the predicted sigma point:

[0081] ;

[0082] Step 4b, Predicting the Mean and Covariance of Prior Measurements:

[0083] ;

[0084] ;

[0085] ;

[0086] Step 4c, Statistical linearization calculation of the measurement slope matrix:

[0087] ;

[0088] in, .

[0089] Step 4d: Adaptively adjust the mixing coefficients of the mixing kernel function based on the EM algorithm:

[0090] Let the current number of EM algorithm iterations be... (initial Based on the current measurement error samples and the mixed kernel function model, the mixing coefficients are iteratively updated through the E-step (expectation step) and M-step (maximization step). , The specific steps are as follows:

[0091] Step E: Calculate the posterior probability

[0092] Based on the current mixing coefficient , With single-mode PDF estimator , Calculate each error sample Posterior probability from the first Gaussian kernel mode The posterior probability from the second Gaussian kernel mode :

[0093] ;

[0094] ;

[0095] M-step: Maximize the log-likelihood estimate of the mixing coefficients

[0096] Posterior probability calculated based on the E-step , The mixing coefficients are updated by maximizing the log-likelihood function of the observed data:

[0097] ;

[0098] ;

[0099] Convergence judgment

[0100] Calculate the absolute value of the difference between the mixing coefficients of two adjacent iterations. ,like If the EM algorithm converges, it will output the final mixing coefficients. , Otherwise, let Return to step E and continue iterating until the maximum number of iterations is reached. .

[0101] Step 4e, EnMMEE cost function optimization: Minimize the EnMMEE cost function using a fixed-point iterative method. The iterative steps are as follows:

[0102] Initialize iteration values: Iteration error Maximum number of iterations ;

[0103] No. iteration :

[0104] 4e-1. Calculate measurement error ;

[0105] 4e-2. Quantification Obtain codebook count Calculate the quantized information potential energy

[0106] 4e-3. Calculate the gradient of the cost function ;

[0107] 4e-4. Update the estimated value using gradient descent: = ,in Step size;

[0108] 4e-5. Calculate the iteration error ,like Less than If the iteration stops, then take... Otherwise, continue iterating until the maximum number of iterations is reached.

[0109] Step 4f, Filter Gain Calculation: Based on the statistical linearization results and the estimated value after EnMMEE optimization, calculate the Kalman gain:

[0110] ;

[0111] Step 4g, posterior state estimation and covariance update:

[0112] ;

[0113] ;

[0114] Step 5, Error Feedback and Iteration: Estimate the posterior state at time k. Covariance and the mixing coefficient after EM algorithm optimization , As Given the initial conditions at time step (2), return to step (2) to perform the filtering iteration at the next time step, thereby achieving continuous state estimation and error correction.

[0115] The present invention, employing the above-described technical solution, has the following beneficial effects: The inertial / acoustic integrated navigation method based on Enhanced Hybrid Minimum Error Entropy / Unscented Kalman Filtering (EnMMEE-UKF) established according to the above steps can effectively address problems such as acoustic ray bending, multimodal noise, and measurement anomalies. By using a mixture of Gaussian kernel functions to model the error distribution and combining it with the EM algorithm to adaptively adjust the mixing coefficients, the estimation accuracy, robustness, and adaptability of the underwater inertial / acoustic integrated navigation system in complex environments are significantly improved while maintaining low computational complexity. Attached Figure Description

[0116] Figure 1 A schematic diagram of the bend of the sound ray provided for the example of this application;

[0117] Figure 2 The EnMMEE-UKF flowchart provided for this application example. Detailed Implementation

[0118] This invention addresses the issues of acoustic ray bending, multimodal noise, and measurement data anomalies in underwater inertial and acoustic integrated navigation systems. By fusing the Enhanced Hybrid Minimum Error Entropy (EnMMEE) criterion with unscented Kalman filtering (UKF), a state estimation framework with both high robustness and low computational complexity is constructed, enabling accurate estimation of the underwater navigation state. The specific method is as follows:

[0119] Step (1): Establish the state equation model for inertial and acoustic integrated navigation, specifically as follows:

[0120] Define the system state vector The state vector includes critical errors in inertial navigation and inherent errors in the sensors:

[0121] ;

[0122] in, This is the carrier attitude misalignment angle error vector, reflecting the deviation between the attitude calculation of the inertial navigation system and the actual attitude; This is the vehicle velocity error vector, describing the difference between the calculated velocity and the actual velocity in inertial navigation. This is the carrier position error vector. This indicates the zero bias of the gyroscope. This indicates the zero bias of the accelerometer;

[0123] Therefore, the state equation for integrated navigation is:

[0124] ;

[0125] in, Represents the state transition matrix. The process noise vector consists of gyroscope random walk, accelerometer random walk, and zero-biased random fluctuation terms, following a Gaussian or Gaussian mixture distribution.

[0126] Step (2): Establish an enhanced hybrid minimum error entropy measurement model that considers the ray bending effect and multimodal noise characteristics, specifically as follows:

[0127] Approximating the underwater sound field as a uniform gradient distribution, the relationship between sound velocity and depth is as follows:

[0128] ;

[0129] in, Indicates depth The speed of sound at the surface of the water is given by g, which represents the speed gradient of sound, and b, which is the speed of sound at the surface of the water.

[0130] Based on Snell's criterion, the propagation of sound rays satisfies the condition that "the ratio of the speed of sound to the cosine of the angle between the sound velocity and the perpendicular to the direction of propagation is constant," expressed as:

[0131] ;

[0132] in, For depth The angle between the sound ray at that point and the direction of the tangent at that point. The angle of incidence of the sound ray at the transponder. The exit angle of the sound ray at the acoustic transducer; and Indicates the depth of the transponder or acoustic transducer;

[0133] Based on ray tracing theory and combined with constant gradient sound velocity profiles, an analytical expression for the propagation time t of sound waves under curved sound lines is derived:

[0134] ;

[0135] in, , , , , This indicates the horizontal distance between the transponder and the acoustic transducer.

[0136] The acoustic system obtains the azimuth angle of the transponder through a transducer array. and Its three-dimensional coordinates relative to the transponder in the transponder coordinate system The relationship is:

[0137] ;

[0138] Based on the Raney entropy theory, the error of The information potential energy of order is defined as:

[0139] ;

[0140] in, For error The probability density function (pdf). For expectation operator, Entropy order, information potential energy It is negatively correlated with error entropy, and maximizes This is equivalent to minimizing the error entropy, which enhances robustness to non-Gaussian noise.

[0141] To adapt to multimodal noise distribution, two Gaussian kernel functions are fused as a mixture kernel for estimating the error pdf, expressed as:

[0142] ;

[0143] in, , The mixing coefficient, , ; , These are two single-modal Parzen window PDF estimators; For Gaussian kernel function, , The kernel width is determined based on the standard deviation of the error samples; The number of error samples. For the first Error samples, .

[0144] Introducing quantization operators reduces the computational complexity of the hybrid minimum error entropy. Quantization operator definition: A scalar quantization operator is used. ,Will Error samples ( )) Quantified to Codebook ,in For the first Each codebook value This is for quantization step size.

[0145] Based on codebook and quantized sample counting, the information potential energy of the hybrid minimum error entropy is simplified to:

[0146] ;

[0147] in, To quantify to the first The number of error samples in each codebook satisfies After quantization, the computational complexity decreased from... Reduce to ,because The computational efficiency is significantly improved.

[0148] By introducing strong tracking characteristics and statistical linearization to optimize robustness, the nonlinear function in the measurement equation is approximated as a linear relationship through statistical linear regression, and the measurement slope matrix is ​​defined.

[0149] ;

[0150] in, For state one-step prediction error The covariance matrix, for The cross-covariance matrix with measurement error.

[0151] By statistical linearization, the nonlinear measurement equation Approximately:

[0152] ;

[0153] in, The measurement noise vector follows a multimodal distribution, and its covariance matrix is... .

[0154] Based on the strong tracking filter (STF) theory, an attenuation factor is introduced. Correcting the state one-step prediction error covariance matrix To enhance the ability to track dynamic disturbances, The calculation method is as follows:

[0155] ;

[0156] in, The actual covariance matrix of the measurement residuals. Here is the theoretical covariance matrix of the measurement residuals; the corrected state one-step prediction error covariance matrix is... .

[0157] The enhanced hybrid minimum error entropy criterion is used as the filtering cost function to replace the traditional UKF mean square error (MSE) cost function. The expression is as follows:

[0158] ;

[0159] in, This is a regularization matrix used to prevent the estimated value from being too large and to constrain the cost function.

[0160] Step (3): Construct the EnMMEE-UKF framework to implement state estimation and error feedback, specifically as follows:

[0161] Step 1: Initialize the filter parameters and state variables to provide initial conditions for the filter iteration; initialize the hybrid kernel function coefficients. Set the convergence threshold for the EM algorithm iteration. With maximum number of iterations .

[0162] Step 2: Generate sigma points using unscented transformation to approximate the probability distribution of the state. The specific steps are as follows:

[0163] Step 2a, calculate The square root matrix of the state estimation at time step: Performing Cholesky decomposition yields... ,satisfy .

[0164] Step 2b, generate sigma points:

[0165] ;

[0166] ;

[0167] ;

[0168] in, express The List.

[0169] Step 3, execute the time update step, and predict. The state and covariance at time t, specifically including:

[0170] Step 3a, one-step prediction of sigma points: Substituting the sigma point at time t into the discretized state equation, we get:

[0171] ;

[0172] in, The process noise sigma point is determined by... The generation follows the same UT transformation rules as the state sigma point.

[0173] Step 3b, Prediction of the prior state mean: The value is predicted in one step by weighted summation of sigma points, yielding:

[0174] ;

[0175] in, The mean weight is used to ensure that the predicted mean is unbiased.

[0176] Step 3c, Predicting the prior error covariance:

[0177] ;

[0178] in, For covariance weights, This represents the process noise covariance matrix.

[0179] Step 3d, Attenuation factor correction: Calculate the attenuation factor at time k based on strong tracking theory. And correct the prior covariance:

[0180] ;

[0181] like Then take ,make sure Zhengding.

[0182] Step 4, execute the measurement update step, combining the EnMMEE criterion and the EM algorithm to optimize the state estimation, specifically including:

[0183] Step 4a, Generate measurement sigma points: for the corrected prior covariance Perform UT transformation to generate measurement sigma points:

[0184] ;

[0185] ;

[0186] in, .

[0187] Substituting the measured sigma point into the nonlinear measurement function, we obtain the predicted sigma point:

[0188] ;

[0189] Step 4b, Predicting the Mean and Covariance of Prior Measurements:

[0190] ;

[0191] ;

[0192] ;

[0193] Step 4c, Statistical linearization calculation of the measurement slope matrix:

[0194] ;

[0195] in, .

[0196] Step 4d: Adaptively adjust the mixing coefficients of the mixing kernel function based on the EM algorithm:

[0197] Let the current number of EM algorithm iterations be... (initial Based on the current measurement error samples and the mixed kernel function model, the mixing coefficients are iteratively updated through the E-step (expectation step) and M-step (maximization step). , The specific steps are as follows:

[0198] Step E: Calculate the posterior probability

[0199] Based on the current mixing coefficient , With single-mode PDF estimator , Calculate each error sample Posterior probability from the first Gaussian kernel mode The posterior probability from the second Gaussian kernel mode :

[0200] ;

[0201] ;

[0202] M-step: Maximize the log-likelihood estimate of the mixing coefficients

[0203] Posterior probability calculated based on the E-step , The mixing coefficients are updated by maximizing the log-likelihood function of the observed data:

[0204] ;

[0205] ;

[0206] Convergence judgment

[0207] Calculate the absolute value of the difference between the mixing coefficients of two adjacent iterations. ,like If the EM algorithm converges, it will output the final mixing coefficients. , Otherwise, let Return to step E and continue iterating until the maximum number of iterations is reached. .

[0208] Step 4e, EnMMEE cost function optimization: Minimize the EnMMEE cost function using a fixed-point iterative method. The iterative steps are as follows:

[0209] Initialize iteration values: Iteration error Maximum number of iterations ;

[0210] No. iteration :

[0211] 4e-1. Calculate measurement error ;

[0212] 4e-2. Quantification Obtain codebook count Calculate the quantized information potential energy

[0213] 4e-3. Calculate the gradient of the cost function ;

[0214] 4e-4. Update the estimated value using gradient descent: = ,in Step size;

[0215] 4e-5. Calculate the iteration error ,like Less than If the iteration stops, then take... Otherwise, continue iterating until the maximum number of iterations is reached.

[0216] Step 4f, Filter Gain Calculation: Based on the statistical linearization results and the estimated value after EnMMEE optimization, calculate the Kalman gain:

[0217]

[0218] Step 4g, posterior state estimation and covariance update:

[0219]

[0220]

[0221] Step 5, Error Feedback and Iteration: Estimate the posterior state at time k. Covariance and the mixing coefficient after EM algorithm optimization , As Given the initial conditions at time step (2), return to step (2) to perform the filtering iteration at the next time step, thereby achieving continuous state estimation and error correction.

[0222] The embodiments described above do not constitute a limitation on the scope of protection of this application.

Claims

1. An inertial and acoustic integrated navigation method based on augmented hybrid minimum error entropy and unscented Kalman filtering, characterized in that, The method is aimed at the problems of sound ray bending, multimodal noise and abnormal measurement data existing in underwater inertial acoustic integrated navigation system, a state estimation framework with high robustness and low computational complexity is constructed by fusing enhanced mixed minimum error entropy criterion and unscented Kalman filter, to realize accurate estimation of underwater navigation state, including: Step (1) establishing the state equation model of inertial and acoustic integrated navigation system; Step (2) establishing an enhanced mixed minimum error entropy measurement model considering the effects of sound ray bending and multimodal noise characteristics, the enhanced mixed minimum error entropy measurement model is based on the construction of curved sound ray propagation model based on isogradient sound speed profile, and introduces mixed kernel function and quantization operator to optimize error entropy calculation to adapt to the complex underwater measurement environment; Step (3) based on the state equation model of step (1) and the enhanced mixed minimum error entropy measurement model of step (2), the EnMMEE-UKF framework is constructed, the error covariance matrix is modified by introducing the decay factor in the strong tracking filter theory, the mixed kernel function mixing coefficient is adaptively adjusted by using the expectation maximization algorithm, and the state estimation and covariance matrix are recursively updated by combining the fixed point iteration method, to realize real-time update and feedback of error.

2. The inertial and acoustic integrated navigation method based on enhanced hybrid minimum error entropy and unscented Kalman filter of claim 1, wherein, In step (1), the state equation model of inertial and acoustic integrated navigation is as follows: Defining the system state vector The state vector includes inertial navigation key errors and sensor inherent errors: ； wherein, is a vector of attitude misalignment angle errors of the carrier, reflecting the deviation of the attitude solution of the inertial navigation system from the real attitude; is a vector of velocity errors of the carrier, describing the difference between the calculated velocity of the inertial navigation and the actual velocity; is a vector of position errors of the carrier, denotes the bias of the gyroscope, denotes the bias of the accelerometer; Therefore, the state equation of integrated navigation is: ； wherein, denotes the state transition matrix, is a process noise vector whose elements are the gyroscope random walk, the accelerometer random walk, and the bias random walk terms, following a Gaussian or a mixture of Gaussians distribution.

3. The inertial and acoustic integrated navigation method based on enhanced hybrid minimum error entropy and unscented Kalman filter of claim 1, wherein, In step (2), the enhanced mixed minimum error entropy measurement model considering the effects of sound ray bending and multimodal noise characteristics is as follows: The underwater acoustic field is approximated as isogradient distribution, and the relationship between sound speed and depth is: ； wherein represents the sound speed at depth g represents the sound speed gradient, and b is the sound speed at the water surface; Based on Snell's law, the sound ray propagation process satisfies "the ratio of sound speed to the cosine value of the angle between propagation direction and vertical line is constant", the expression is: ； wherein, is the depth of the acoustic transducer, is the angle between the sound ray and the tangent direction at the point, is the angle of incidence of the sound ray at the transponder, is the angle of emergence of the sound ray at the acoustic transducer; and denotes the depth of the transponder, the acoustic transducer. Based on the ray tracing theory, combined with the isogradient sound speed profile, the analytical expression of sound wave propagation time t under curved sound ray is derived: ； wherein , , , , represents the horizontal distance between the transponder, the acoustic transducer. An acoustic system acquires the bearing of a transponder by means of a transducer array With The relationship of the three-dimensional coordinates of the transponder in the transducer coordinate system is ； Based on the Renyi entropy theory, the error of the order information potential energy is defined as: ； wherein, is the error a probability density function (pdf) of the error is the expectation operator, is the entropy order, information potential is negatively correlated with the error entropy, maximizing is equivalent to minimizing the error entropy, which can enhance the robustness to non-Gaussian noise; In order to adapt to the multimodal noise distribution, two Gaussian kernel functions are fused as mixed kernel to estimate the error pdf, the expression is: ； wherein, , is a mixing coefficient, , ; , are two unimodal Parzen window pdf estimators, respectively; is a Gaussian kernel function, , is a kernel width determined from the error sample standard deviation; is the number of error samples, is the error sample, ; The quantization operator is introduced to reduce the computational complexity of the hybrid minimum error entropy, and the quantization operator is defined as follows: a scalar quantization operator , and error samples ( ) are quantized into codebooks , where is the th codebook value, and is the quantization step size; Based on codebook and quantization sample count, the information potential energy of mixed minimum error entropy is simplified as: ； Wherein, is the number of error samples quantized to the first codebook, satisfies ; the calculation complexity after quantization is reduced from to , and due to , the calculation efficiency is significantly improved; By introducing the strong tracking characteristics and optimizing the robustness of statistical linearization, the nonlinear function in the measurement equation is approximated to linear relationship by statistical linear regression, and the measurement slope matrix is defined as: ； wherein, is a step ahead prediction error of the state is a covariance matrix of the state prediction error, is is a cross covariance matrix with the measurement error; By statistical linearization, the nonlinear measurement equation is approximated as: ； wherein is a vector of measurement noise, following a multi-modal distribution, with covariance matrix ; Based on the strong tracking filter theory, a damping factor is introduced to modify the one-step prediction error covariance matrix to enhance the tracking ability of dynamic disturbances, The calculation method is as follows: ； wherein, is the actual covariance matrix of the measurement residuals, is the theoretical covariance matrix of the measurement residuals; the corrected state one-step prediction error covariance matrix is ; The enhanced mixed minimum error entropy criterion is used as the filter cost function to replace the traditional UKF mean square error cost function, the expression is: ； wherein is a regularization matrix, which prevents overestimation of the estimates and constrains the cost function.

4. The inertial and acoustic integrated navigation method based on enhanced hybrid minimum error entropy and unscented Kalman filter of claim 1, wherein, In step (3), the EnMMEE-UKF framework is constructed to realize state estimation and error feedback, including: Step 1, initialize filter parameters and state variables to provide initial conditions for the filter iteration; initialize the mixing kernel function coefficients , set the EM algorithm iteration convergence threshold and the maximum number of iterations ; Step 2, sigma points are generated by unscented transformation to approximate the probability distribution of state, the specific steps are as follows: Step 2a, compute the square root matrix of the time instant state estimate: Q perform Cholesky decomposition, obtaining , satisfying ; Step 2b, generation Sigma points: ； ； ； wherein represents the first column; Step 3, perform time update step, predict the state and covariance at time, in particular comprising: Step 3a, sigma point one-step prediction: Substitute the sigma points at time k into the discretized state equation to obtain: Substitute the sigma points at time k into the discretized state equation to obtain: ； wherein are process noise sigma points generated, following the same UT transformation rules as the state sigma points; and are process noise sigma points generated, following the same UT transformation rules as the state sigma points; and Step 3b, prior state mean prediction: the one-step predicted value of sigma point is obtained by weighted summation: ； wherein, is the mean weight, ensuring that the predicted mean is unbiased; Step 3c, prior error covariance prediction: ； wherein is a covariance weight, denotes a process noise covariance matrix; Step 3d, decay factor correction: compute the decay factor at time k according to the strong tracking theory and correct the prior covariance: ； If , take , ensure positive definite; Step 4, execute measurement update step, combine EnMMEE criterion and EM algorithm to optimize state estimation, including: Step 4a, generate measurement sigma points: on the corrected prior covariance Perform UT transform, generate measurement sigma points: ； ； wherein ; Substitute the measurement sigma point into the nonlinear measurement function to get the measurement predicted sigma point: ； Step 4b, prior measurement mean and covariance prediction: ； ； ； Step 4c, Statistically linearized computation of measurement Jacobian matrix: ； wherein ; Step 4d, Adaptive adjustment of the mixing coefficients of the hybrid kernel function based on EM algorithm: Let the current iteration number of EM algorithm be (initial )Based on the current measurement error sample and the mixed kernel function model, the mixed coefficient is updated through E step and M step iteration 、 The specific steps are as follows: E-step: Compute posterior probability According to the current mixing coefficients , and the unimodal pdf estimators , , compute the posterior probability from the first Gaussian kernel mode from the second Gaussian kernel mode : ； ； M-step: Maximize log-likelihood estimate mixing coefficients Posterior probabilities based on E-step calculations , Update the mixing coefficients by maximizing the log-likelihood function of the observed data: ； ； Convergence judgment Calculate the absolute value of the difference between the mixing coefficients of two adjacent iterations , if , the EM algorithm converges, and the final mixing coefficients are output , ; otherwise, let , return to the E step for iteration until the maximum number of iterations is reached ; Step 4e, EnMMEE cost function optimization: Minimize the EnMMEE cost function using a fixed point iteration method The iteration steps are as follows: Initialize iteration value: , iteration error , maximum iteration number ; first iteration second iteration : 1e. Calculate measurement error ; 2e. quantization obtaining a codebook count calculating a post-quantization information potential ; 3e. Computing the cost function gradient ; 4e. Update the estimates using gradient descent: = where is the step size; 5e. Compute iteration error , if is less than , then stop iteration and take ; otherwise continue iteration until maximum number of iterations is reached; Step 4f, Filter gain computation: Compute Kalman gain based on the results of statistical linearization and the optimized estimate value of EnMMEE: ； Step 4g, Posterior state estimation and covariance update: ； ； Step f, Error Feedback and Iteration: Estimate the posterior state at time k Covariance and the mixing coefficient after EM algorithm optimization , As Given the initial conditions at time step (2), return to step (2) to perform the filtering iteration at the next time step, thereby achieving continuous state estimation and error correction.

Images