Interactive multi-model navigation filtering method based on quasi-Newton optimization

Abstract

The invention discloses an interactive multi-model navigation filtering method based on quasi-Newton optimization, and belongs to an underwater robot integrated navigation positioning technology. According to the method, outlier point judgment is carried out based on residual characteristic analysis; rejecting and replacing the judged outlier points based on a sliding window to obtain corrected measurement data; carrying out noise covariance matrix identification on the obtained corrected measurement data based on a quasi-Newton algorithm; and finally, carrying out state fusion on the identified noise covariance matrix based on an interactive Kalman filtering algorithm. The method can effectively solve the problems of unstable sensor information, time-varying noise, motion mode switching and the like of the underwater robot, and compared with a traditional method, the method has the advantage that the anti-interference capability and the positioning precision of a navigation system are remarkably improved.

Description

Technical Field

[0001] This invention belongs to the field of underwater robot integrated navigation and positioning technology, specifically involving an interactive multi-model navigation filtering method based on quasi-Newton optimization. Background Technology

[0002] Underwater robots play a crucial role in marine resource exploration, deep-sea mineral development, underwater pipeline inspection, and scientific research in complex terrain. The accuracy and reliability of their operations heavily rely on information provided by auxiliary sensors. However, when underwater robots perform engineering operations or tasks underwater, the effectiveness of the auxiliary sensor information they receive often changes with the working environment and the vehicle's motion patterns, exhibiting unpredictability and even signal interruptions. These intertwined factors pose a significant challenge to the effectiveness of auxiliary sensor information. Current mainstream integrated navigation algorithms face multiple technical bottlenecks in underwater robot navigation applications: traditional Kalman filtering is sensitive to outliers, interactive multi-model algorithms struggle to match unknown motion patterns, noise-adaptive algorithms suffer from lag in parameter updates, and existing technologies have shortcomings in handling time-varying noise, nonlinear dynamics, and sensor anomalies. Therefore, there is an urgent need for navigation algorithms that combine anti-interference and adaptive optimization mechanisms. Summary of the Invention

[0003] Purpose of the invention: To address the above problems, this invention proposes an interactive multi-model combined navigation filtering method based on quasi-Newton optimization to improve the anti-interference capability and positioning accuracy of underwater robot navigation systems.

[0004] The above objectives are achieved through the following technical solutions:

[0005] The present invention provides an interactive multi-model combined navigation filtering method based on quasi-Newton optimization, which is based on the following known quantities:

[0006] Three-dimensional velocity information output by Doppler log (DVL) ,in, , , These represent the velocities in the x, y, and z directions of the Doppler log DVL measurement coordinate system, respectively. The superscript T indicates transpose, which converts the row vector into a column vector.

[0007] Measurement matrix ,in This represents the attitude matrix from the world coordinate system to the DVL measurement coordinate system. This represents the compensation term for the link arm error caused by the carrier's angular velocity. This is used to correlate the three-dimensional velocity information output by the Doppler log with the system's measurement model, enabling the effective use of velocity information in navigation filtering. Its expression is as follows: ;

[0008] The method includes the following steps:

[0009] Step 1: Identify outlier points based on residual characteristic analysis;

[0010] Step 2: Remove and replace outlier points identified in Step 1 using a sliding window to obtain corrected measurement data;

[0011] Step 3: Identify the noise covariance matrix of the corrected measurement data obtained in Step 2 based on the quasi-Newton algorithm;

[0012] Step 4: Perform state fusion on the noise covariance matrix identified in Step 3 using the interactive Kalman filter algorithm.

[0013] Furthermore, the specific method for step 1 is as follows:

[0014] Take the residual Subtract the difference between the measurement vector and the product of the measurement matrix and the predicted value: , in, express The measurement vector at time t. express Measurement matrix at time, Indicates based on Information about time is important The predicted value of the system state at any given time;

[0015] In the ideal case with no outliers, the residual values ​​conform to the Markov property and can be described by a second-order AR model: ,, in, It is a random sequence describing the residuals. These are the coefficients of the second-order AR model. express The residual random sequence values ​​at time t. express The residual random sequence values ​​at time t. This represents a white noise sequence with a mean of 0.

[0016] Calculate the mean of the residuals :

[0017] in, This represents the values ​​of the residual random sequence at time b;

[0018] Pick The residual statistics at the current time: , in, This represents the mean residual at time k-1. This represents the mean residual at time k-2;

[0019] The introduced length is Sliding window, calculation mean Standard deviation If satisfied ,in, The threshold coefficient represents the value at which a measurement is considered an outlier; otherwise, it is not considered an outlier.

[0020] Furthermore, in step 2, the outlier points identified in step 1 are eliminated and replaced based on a sliding window, depending on the time when the outlier points appear. Less than the length of the sliding window Or the moment when a wild point appears Greater than or equal to the sliding window length The elimination and replacement are divided into two different cases, and the specific methods are as follows:

[0021] ①When outlier points appear Less than the length of the sliding window At this point, the amount of data is insufficient, so an adjustment coefficient is introduced. Correct the original data corresponding to the outlier points. , :

[0022] in, Here, t is a constant determined based on the actual situation, and t is the index variable for the summation operation. This is used to iterate through the integer sequence from k-m+1 to k, and accumulate the statistics of the most recent m samples. express Residual statistics at time points. Indicates the window The mean;

[0023] New data after correction : ;

[0024] ②When outlier points appear Greater than or equal to the sliding window length Let the weighted sequence be ,satisfy Set a forgetting factor ,get ;

[0025] New data after correction : , express The measurement vector at time.

[0026] Furthermore, the specific method for step 3 is as follows:

[0027] After processing in steps 1 and 2, the residual for ;

[0028] Construct the objective function : , Where N represents the time series length of the Kalman filter, and the objective function is... Minimizing the overall residual corresponds to minimizing the noise impact, providing a clear objective for optimizing the filter parameters; by adjusting the objective function F on the th The parameter vector to be optimized in the next iteration Calculate the gradient by taking partial derivatives Based on the step-by-step derivation of the Kalman filter formula, the following is obtained: , in, express The measurement noise covariance matrix at time 1. express The process noise covariance matrix at time step 1. Indicates based on Information about time is important The calculated value of the system state at any given time;

[0029] Check the norm of the gradient :

[0030] like If the calculation stops, output the current vector of parameters to be optimized. As an optimized and The estimated value, of which, This represents the convergence threshold at which the quasi-Newton algorithm terminates its iterations. Represents the process noise covariance matrix. Represents the measurement noise covariance matrix;

[0031] like Then proceed to the next step: solve the system of linear equations. Get search direction As an iteration to determine the direction, where... In the quasi-Newton algorithm, the first... The Hessian approximation matrix at the next iteration is used to approximate the Hessian matrix of the objective function;

[0032] Determining the step size using the Wolfe line search criterion According to the determined step size and search direction Calculate the parameter vector to be optimized at the new iteration point. : ,

[0033] Through the objective function For the first The parameter vector to be optimized in the next iteration Calculate the gradient by taking partial derivatives ,like If the calculation fails, stop; otherwise, correct the error. , Indicates the first The updated Hessian approximation matrix in the next iteration.

[0034] Furthermore, the specific method for step 4 is as follows:

[0035] S41. To improve the accuracy of state estimation for nonlinear states using unscented Kalman filtering, first, a set of sigma points and corresponding weights are generated: , in, express The first moment 1 Sigma point, express State estimate at time 10:00 express The covariance matrix at time t, The dimension of the system state vector. The scaling parameter represents the unscented transformation;

[0036] Perform another unscented transformation to generate the updated sigma point set: , in, This represents the i-th sigma point generated at time k+1 based on the information at time k. This represents the predicted system state at time k+1 based on the information at time k. This represents the predicted value of the system covariance matrix at time k+1 based on the information at time k.

[0037] Calculate the Kalman gain matrix at time k+1 : , in, Represents the covariance of the observed values. This represents the correlation covariance between the state vector and the observed values;

[0038] Calculate the updated covariance matrix: , in, Let T represent the system covariance matrix updated at time k+1 based on the information at time k+1, and the superscript T indicates the transpose of the matrix;

[0039] S42. To improve the robustness of the system to sudden noise changes by using extended-dimensional Kalman filtering, firstly, a diagonal array is used. Reconstruct the state prediction covariance matrix, where, This indicates adjusting the scaling parameter of the state prediction covariance matrix. for: , in, This represents the original state prediction covariance matrix at time k+1;

[0040] The Kalman gain is: , in, This represents the Kalman gain matrix at time k+1. Represents the measurement matrix. Represents the measurement noise covariance matrix;

[0041] The covariance matrix is: , in, This represents the state prediction covariance matrix updated at time k+1;

[0042] S43. To achieve adaptive switching and state fusion of sub-filters using interactive Kalman filtering, first define the system's model set. : , in, Indicates the first Sub-model;

[0043] Likelihood function: , in, Let k represent the measurement noise covariance matrix at time k. Pi is a mathematical constant. Represents the model at time k. The likelihood function;

[0044] Secondly, the likelihood function is used. Determine the model update probability: , in, Represents the model at time k. The update probability, express Time model The prior mixture probability, express Time model The prior mixture probability;

[0045] Finally, an estimation fusion is performed: , , in, express Fusion state estimation of the time-series system Represents the model at time k. The update probability, express Time model Posterior state estimation, express The fused post-hoc covariance of the time-series systems This indicates the number of sub-models participating in the fusion. express Time model The posterior covariance, Indicates in Time model The prior state estimate of itself obtained after fusing the posterior state estimates of other sub-models.

[0046] Beneficial effects:

[0047] 1. This invention addresses the problem of Kalman filtering being susceptible to numerical interference from outliers by introducing a second-order AR model to approximate the residuals and combining it with the Wright criterion to identify outliers. Furthermore, it addresses the relationship between the occurrence time of outliers and the length of the sliding window by using adjustment coefficient correction and weighted averaging of sliding window data to process outliers, thereby improving the noise interference removal effect.

[0048] 2. This invention addresses the problem of identifying the process noise covariance matrix Q and the measurement noise covariance matrix R. It defines the sum of squared residuals as the objective function and uses the BFGS quasi-Newton algorithm to optimize the objective function, thereby improving the adaptive capability of the filtering parameters and the accuracy of the navigation system.

[0049] 3. This invention addresses the problem of complex nonlinear underwater environments by introducing traditional sub-filters, unscented sub-filters, and extended-dimensional sub-filters. It also introduces the sub-filter transition probability and the conditional probability of sub-filter occurrence to achieve interactive Kalman filtering, thereby suppressing uncertainty errors in complex time-varying underwater environments. Attached Figure Description

[0050] Figure 1 This is a flowchart illustrating the outlier point determination based on residual characteristic analysis as described in this invention.

[0051] Figure 2 This is a flowchart of outlier removal and replacement based on a sliding window, as described in this invention.

[0052] Figure 3 This is a flowchart of the interactive multi-model filtering based on quasi-Newton optimization described in this invention;

[0053] Figure 4 The trajectory curve of the test vessel of this invention;

[0054] Figure 5 The results show the position error curves of the traditional method and the method of this invention in different directions.

[0055] Figure 6 The results show the horizontal position error curves of the traditional method and the method of this invention;

[0056] Figure 7 This is a flowchart of the method of the present invention. Detailed Implementation

[0057] The interactive multi-model combined navigation filtering method based on quasi-Newton optimization in this embodiment is based on the following known quantities:

[0058] Three-dimensional velocity information output by Doppler log (DVL) ,in, , , These represent the velocities in the x, y, and z directions of the Doppler log DVL measurement coordinate system, respectively. The superscript T indicates transpose, which converts the row vector into a column vector.

[0059] Measurement matrix ,in This represents the attitude matrix from the world coordinate system to the DVL measurement coordinate system. This represents the compensation term for the link arm error caused by the carrier's angular velocity. This is used to correlate the three-dimensional velocity information output by the Doppler log with the system's measurement model, enabling the effective use of velocity information in navigation filtering. Its expression is as follows: ;

[0060] like Figure 7 As shown, the method of the present invention specifically includes the following steps:

[0061] Step 1: Identification of outlier points based on residual characteristic analysis, such as... Figure 1 As shown, in order to effectively remove noise interference, this invention designs an outlier point judgment method based on residual characteristic analysis.

[0062] Take the residual Subtract the difference between the measurement vector and the product of the measurement matrix and the predicted value: , in, express The measurement vector at time t. express Measurement matrix at time, Indicates based on Information about time is important The predicted value of the system state at any given time;

[0063] In the ideal case with no outliers, the residual values ​​conform to the Markov property and can be described by a second-order AR model: , in, It is a random sequence describing the residuals. These are the coefficients of the second-order AR model. express The residual random sequence values ​​at time t. express The residual random sequence values ​​at time t. This represents a white noise sequence with a mean of 0.

[0064] To effectively remove noise interference, the mean residual value is calculated. : , in, This represents the values ​​of the residual random sequence at time b;

[0065] Pick The residual statistics at the current time: , in, This represents the mean residual at time k-1. This represents the mean residual at time k-2;

[0066] The introduced length is Sliding window, calculation mean Standard deviation If satisfied ,in, This represents the threshold coefficient for identifying outliers, in this embodiment... If the value is 3, the corresponding measurement value is determined to be an outlier; otherwise, it is not an outlier.

[0067] Step 2: Outlier removal and replacement based on a sliding window, such as... Figure 2 As shown, to improve the flexibility and timeliness of outlier removal and replacement, this invention designs an outlier removal and replacement method based on a sliding window, which requires consideration of the time when the outlier appears. Less than the length of the sliding window Or the moment when a wild point appears Greater than or equal to the sliding window length The elimination and replacement are divided into two different cases, and the specific methods are as follows:

[0068] ①When outlier points appear Less than the length of the sliding window At this point, the amount of data is insufficient, so an adjustment coefficient is introduced. Correct the original data corresponding to the outlier points. , : , in, Here, t is a constant determined based on the actual situation, and t is the index variable for the summation operation. This is used to iterate through the integer sequence from k-m+1 to k, and accumulate the statistics of the most recent m samples. express Residual statistics at time points. Indicates the window Mean; adjustment coefficient Using data within the window for calculations can both reduce the impact of outliers and avoid information loss caused by directly removing outliers.

[0069] New data after correction :

[0070] ②When outlier points appear Greater than or equal to the sliding window length Let the weighted sequence be ,satisfy Set a forgetting factor (In this embodiment) A value of 0.97 ensures that the new data has a higher weighting and is more sensitive to changes in noise, resulting in... ;

[0071] New data after correction : , express The measurement vector at time.

[0072] Step 3: Identify the noise covariance matrix of the corrected measurement data obtained in Step 2 using a quasi-Newton algorithm, such as... Figure 3 As shown, in order to achieve adaptive updating of noise parameters and improve the adaptive capability of filtering parameters and the accuracy of navigation system, this invention designs a noise identification method based on quasi-Newton algorithm.

[0073] After processing in steps 1 and 2, the residual for: ,

[0074] Construct the objective function : , Where N represents the time series length of the Kalman filter, and the objective function is... Minimizing the overall residual corresponds to minimizing the noise impact, providing a clear objective for optimizing the filter parameters; by adjusting the objective function F on the th The parameter vector to be optimized in the next iteration Calculate the gradient by taking partial derivatives Based on the step-by-step derivation of the Kalman filter formula, the following is obtained: , in, express The measurement noise covariance matrix at time 1. express The process noise covariance matrix at time step 1. Indicates based on Information about time is important The calculated value of the system state at any given time;

[0075] Check the norm of the gradient :

[0076] like If the calculation stops, output the current vector of parameters to be optimized. As an optimized and The estimated value, of which, This represents the convergence threshold at which the quasi-Newton algorithm terminates its iterations. Represents the process noise covariance matrix. Represents the measurement noise covariance matrix;

[0077] like Then proceed to the next step: solve the system of linear equations. Get search direction As an iteration to determine the direction, where... In the quasi-Newton algorithm, the first... The Hessian approximation matrix at the next iteration is used to approximate the Hessian matrix of the objective function;

[0078] Determining the step size using the Wolfe line search criterion According to the determined step size and search direction Calculate the parameter vector to be optimized at the new iteration point. : , Through the objective function For the first The parameter vector to be optimized in the next iteration Calculate the gradient by taking partial derivatives ,like If the calculation fails, stop; otherwise, correct the error. , Indicates the first The updated Hessian approximation matrix in the next iteration.

[0079] Step 4: Perform state fusion on the noise covariance matrix identified in Step 3 using the interactive Kalman filter algorithm.

[0080] To adapt to the switching of different sub-filters and suppress uncertainty errors in complex time-varying underwater environments, this invention designs an interactive Kalman filter.

[0081] S41. To improve the accuracy of state estimation for nonlinear states using unscented Kalman filtering, first, a set of sigma points and corresponding weights are generated: , in, express The first moment 1 Sigma point, express State estimate at time 10:00 express The covariance matrix at time t, The dimension of the system state vector. The scaling parameter represents the unscented transformation;

[0082] Perform another unscented transformation to generate the updated sigma point set: , in, This represents the i-th sigma point generated at time k+1 based on the information at time k. This represents the predicted system state at time k+1 based on the information at time k. This represents the predicted value of the system covariance matrix at time k+1 based on the information at time k.

[0083] By generating sigma points and performing an unscented transformation, linearization of the nonlinear system model is unnecessary, avoiding the linearization error of traditional extended Kalman filtering, and allowing for a more accurate approximation of the probability distribution of the system state; the Kalman gain matrix at time k+1 is calculated. : , in, Represents the covariance of the observed values. This represents the correlation covariance between the state vector and the observed values;

[0084] Calculate the updated covariance matrix: , in, Let T represent the system covariance matrix updated at time k+1 based on the information at time k+1, and the superscript T indicates the transpose of the matrix;

[0085] S42. To improve the robustness of the system to sudden noise changes by using extended-dimensional Kalman filtering, firstly, a diagonal array is used. Reconstruct the state prediction covariance matrix, where, This indicates adjusting the scaling parameter of the state prediction covariance matrix. for: , in, This represents the original state prediction covariance matrix at time k+1;

[0086] The Kalman gain is: , in, This represents the Kalman gain matrix at time k+1. Represents the measurement matrix. Represents the measurement noise covariance matrix;

[0087] The covariance matrix is: , in, This represents the state prediction covariance matrix updated at time k+1. The adjusted Kalman gain matrix and covariance matrix can more flexibly integrate measurement information, reduce filter divergence caused by drastic noise changes, and improve the system's adaptability to sudden noise changes.

[0088] S43. To achieve adaptive switching and state fusion of sub-filters using interactive Kalman filtering, first define the system's model set. : , in, Indicates the first Each sub-model is designed for different characteristics of the underwater environment, and the model set ensures full coverage of complex underwater conditions such as stable noise, strong nonlinearity, and sudden noise changes.

[0089] Likelihood function: , in, Let k represent the measurement noise covariance matrix at time k. Pi is a mathematical constant. Represents the model at time k. The likelihood function;

[0090] Secondly, the likelihood function is used. Determine the model update probability: , in, Represents the model at time k. The update probability, express Time model The prior mixture probability, express Time model The prior mixture probability; the model update probability is key to achieving adaptive switching of sub-filters. When the underwater environment changes, the likelihood function value of the corresponding sub-model will increase significantly, and its corresponding Increased, it dominates in multi-model filters.

[0091] Finally, an estimation fusion is performed: , , in, express Fusion state estimation of the time-series system Represents the model at time k. The update probability, express Time model Posterior state estimation, express The fused post-hoc covariance of the time-series systems This indicates the number of sub-models participating in the fusion. express Time model The posterior covariance, Indicates in Time model The prior state estimate of itself obtained after fusing the posterior state estimates of other sub-models.

[0092] Experimental verification

[0093] This invention uses experimental data from SINS / DVL integrated navigation conducted in Qiandao Lake, Hangzhou, for analysis in MATLAB. The experimental system platform includes a DVL device, an inertial measurement unit (IMU), an RTK GPS device, an embedded navigation computer, and a debugging computer. First, the IMU, DVL, and RTK GPS devices are fixed together by mounting rods. Second, the DVL device is mounted at one end of a U-shaped bracket, the IMU is mounted near the center of the U-shaped bracket, and the RTK GPS receiver is mounted directly above the IMU. Finally, after pre-measuring the error parameters of each rod arm, the entire mounting rod is fixed to one side of the experimental vessel's hull and submerged underwater.

[0094] The DVL device has an update frequency of 2Hz and a scaling factor of 0.004; the gyroscope bias drift is 0.02° / h, and the random walk noise is... The accelerometer bias drift is 50 μg, and the random walk noise is... The IMU update frequency is 200Hz. The navigation computer collects DVL device data and IMU data in real time and completes navigation calculations.

[0095] Based on the above experimental conditions, Figure 4 This is the motion trajectory curve of the test vessel of this invention. Based on the above trajectory, Figure 5 The figures show the position error curves of the traditional method and the method of this invention in different directions. It can be seen from the figures that the method of this invention has a smaller positioning error. For a more intuitive understanding, Figure 6 The horizontal position error curves of the conventional method and the method of this invention are presented. The figures show that the accuracy of the method of this invention is superior to that of the conventional method.

Claims

1. An interactive multi-model navigation filtering method based on quasi-Newton optimization, the method being based on the following known quantities: Three-dimensional velocity information output by Doppler log (DVL) ,in, , , These represent the velocities in the x, y, and z directions of the Doppler log DVL measurement coordinate system, respectively. The superscript T indicates transpose, which converts the row vector into a column vector. Measurement matrix ,in This represents the attitude matrix from the world coordinate system to the DVL measurement coordinate system. This represents the compensation term for the link arm error caused by the carrier's angular velocity. This is used to correlate the three-dimensional velocity information output by the Doppler log with the system's measurement model, enabling the effective use of velocity information in navigation filtering. Its expression is as follows: ; The method is characterized by comprising the following steps: Step 1: Identify outlier points based on residual characteristic analysis; Step 2: Remove and replace outlier points identified in Step 1 using a sliding window to obtain corrected measurement data; Step 3: Identify the noise covariance matrix of the corrected measurement data obtained in Step 2 based on the quasi-Newton algorithm; Step 4: Perform state fusion on the noise covariance matrix identified in Step 3 using the interactive Kalman filter algorithm.

2. The interactive multi-model navigation filtering method based on quasi-Newton optimization according to claim 1, characterized in that, The specific method for step 1 is as follows: Take the residual Subtract the difference between the measurement vector and the product of the measurement matrix and the predicted value: ， in, express The measurement vector at time t. express Measurement matrix at time, Indicates based on Information about time is important The predicted value of the system state at any given time; In the ideal case with no outliers, the residual values ​​conform to the Markov property and can be described by a second-order AR model: ， in, It is a random sequence describing the residuals. These are the coefficients of the second-order AR model. express The residual random sequence values ​​at time t. express The residual random sequence values ​​at time t. This represents a white noise sequence with a mean of 0. Calculate the mean of the residuals : ， in, This represents the values ​​of the residual random sequence at time b; Pick The residual statistics at the current time: ， in, This represents the mean residual at time k-1. This represents the mean residual at time k-2; The introduced length is Sliding window, calculation mean Standard deviation If the conditions are met ,in, The threshold coefficient represents the value at which a measurement is considered an outlier; otherwise, it is not considered an outlier.

3. The interactive multi-model navigation filtering method based on quasi-Newton optimization according to claim 2, characterized in that, Step 2 involves removing and replacing outliers identified in Step 1 using a sliding window method, depending on the timing of their occurrence. Less than the length of the sliding window Or the moment when a wild point appears Greater than or equal to the sliding window length The elimination and replacement are divided into two different cases, and the specific methods are as follows: ①When outlier points appear Less than the length of the sliding window At this point, the amount of data is insufficient, so an adjustment coefficient is introduced. Correct the original data corresponding to the outlier points. , : ， in, Here, t is a constant determined based on the actual situation, and t is the index variable for the summation operation. This is used to iterate through the integer sequence from k-m+1 to k, and accumulate the statistics of the most recent m samples. express Residual statistics at time points. Indicates the window The mean; New data after correction : ， ②When outlier points appear Greater than or equal to the sliding window length Let the weighted sequence be ,satisfy Set a forgetting factor ,get ; New data after correction : ， express The measurement vector at time.

4. The interactive multi-model navigation filtering method based on quasi-Newton optimization according to claim 3, characterized in that, The specific method for step 3 is as follows: After processing in steps 1 and 2, the residual for , Construct the objective function : ， Where N represents the time series length of the Kalman filter, and the objective function is... Minimizing the overall residual corresponds to minimizing the noise impact, providing a clear objective for optimizing the filter parameters; by adjusting the objective function F on the th The parameter vector to be optimized in the next iteration Calculate the gradient by taking partial derivatives Based on the step-by-step derivation of the Kalman filter formula, the following is obtained: ， in, express The measurement noise covariance matrix at time 1. express The process noise covariance matrix at time step 1. Indicates based on Information about time is important The calculated value of the system state at any given time; Check the norm of the gradient : like If the calculation stops, output the current vector of parameters to be optimized. As an optimized and The estimated value, of which, This represents the convergence threshold at which the quasi-Newton algorithm terminates its iterations. Represents the process noise covariance matrix. Represents the measurement noise covariance matrix; like Then proceed to the next step: solve the system of linear equations. Get search direction As an iteration to determine the direction, where... In the quasi-Newton algorithm, the first... The Hessian approximation matrix at the next iteration is used to approximate the Hessian matrix of the objective function; Determining the step size using the Wolfe line search criterion According to the determined step size and search direction Calculate the parameter vector to be optimized at the new iteration point. : ， Through the objective function For the The parameter vector to be optimized in the next iteration Calculate the gradient by taking partial derivatives ,like If the calculation fails, stop; otherwise, correct the error. , Indicates the first The updated Hessian approximation matrix in the next iteration.

5. The interactive multi-model navigation filtering method based on quasi-Newton optimization according to claim 4, characterized in that, The specific method for step 4 is as follows: S41. To improve the accuracy of state estimation for nonlinear states using unscented Kalman filtering, first, a set of sigma points and corresponding weights are generated: ， in, express The first moment 1 Sigma point, express State estimate at time 10:00 express The covariance matrix at time t, The dimension of the system state vector. The scaling parameter represents the unscented transformation; Perform another unscented transformation to generate the updated sigma point set: ， in, This represents the i-th sigma point generated at time k+1 based on the information at time k. This represents the predicted system state at time k+1 based on the information at time k. This represents the predicted value of the system covariance matrix at time k+1 based on the information at time k. Calculate the Kalman gain matrix at time k+1 : ， in, Represents the covariance of the observed values. This represents the correlation covariance between the state vector and the observed values; Calculate the updated covariance matrix: ， in, Let T represent the system covariance matrix updated at time k+1 based on the information at time k+1, and the superscript T indicates the transpose of the matrix; S42. To improve the robustness of the system to sudden noise changes by using extended-dimensional Kalman filtering, firstly, a diagonal array is used. Reconstruct the state prediction covariance matrix, where, This indicates adjusting the scaling parameter of the state prediction covariance matrix. for: ， in, This represents the original state prediction covariance matrix at time k+1; The Kalman gain is: ， in, This represents the Kalman gain matrix at time k+1. Represents the measurement matrix. Represents the measurement noise covariance matrix; The covariance matrix is: ， in, This represents the state prediction covariance matrix updated at time k+1; S43. To achieve adaptive switching and state fusion of sub-filters using interactive Kalman filtering, first define the system's model set. : ， in, Indicates the first Sub-model; Likelihood function: ， in, Let k represent the measurement noise covariance matrix at time k. Pi is a mathematical constant. Represents the model at time k. The likelihood function; Secondly, the likelihood function is used. Determine the model update probability: ， in, Represents the model at time k. The update probability, express Time model The prior mixture probability, express Time model The prior mixture probability; Finally, an estimation fusion is performed: ， ， in, express Fusion state estimation of the time-series system Represents the model at time k. The update probability, express Time model Posterior state estimation, express The fused post-hoc covariance of the time-series systems This indicates the number of sub-models participating in the fusion. express Time model The posterior covariance, Indicates in Time model The prior state estimate of itself obtained after fusing the posterior state estimates of other sub-models.

Images