GS-STSCKF and credibility theory coupling-based multi-algorithm fusion USV state estimation method for nonlinear non-Gaussian system

Abstract

The invention discloses a GS-STSCKF and credibility theory coupling-based multi-algorithm fusion USV state estimation method for a nonlinear non-Gaussian system, and the method comprises the steps: inputting the measurement information of a sensor into a credible GS-STSCKF algorithm, and obtaining a filtering result; meanwhile, a filtered state estimation value and a difference value of state one-step prediction are input into the improved BLS network, and an error between a filtering result and an actual value is output; and summing the filtering result output by the credible GS-STSCKF algorithm and the output of the improved BLS network to obtain an estimation state value of the USV.

Description

Technical Field

[0001] The present invention belongs to the technical field of USV state estimation, and particularly relates to a multi-algorithm fusion USV state estimation method based on the coupling of GS-STSCKF and credibility theory for non-linear and non-Gaussian systems. Background Art

[0002] Unmanned Surface Vehicles (USVs) are playing an increasingly important role in marine applications such as environmental monitoring, ocean patrol, and search and rescue. The accurate estimation of their motion states is crucial for effective operation and directly affects navigation safety, stability, and overall performance.

[0003] The motion state estimation of USVs involves the real-time determination of the position, velocity, and direction of USVs. In a dynamic and unpredictable marine environment, this task is full of challenges. Factors such as waves, wind, and water currents make it difficult for traditional estimation methods to provide the required accuracy and reliability. Therefore, improving the accuracy of USV motion state estimation is of great significance for enhancing its autonomy and operation efficiency. Factors such as waves, wind, and water currents in the marine environment are random and variable, resulting in complex non-linear dynamic systems and making the motion trajectories of USVs difficult to predict. The instantaneous change characteristics such as sudden storms, turbulence, and surges further increase the difficulty of state estimation. These dynamic and complex factors often deviate from the Gaussian distribution, leading to an increase in the complexity of environmental noise and interference. In addition, equipment measurement errors will also introduce more noise, further deviating from the Gaussian distribution, thus increasing the difficulty of USV motion state estimation.

[0004] Existing state estimation methods such as strong tracking square root cubature Kalman filter (STSCKF) often degrade in performance when faced with non-Gaussian noise and cannot provide accurate and reliable results. Therefore, continuous innovation in theory and methods is needed to overcome these challenges and enhance the autonomy and operation efficiency of USVs.

[0005] The existing STSCKF algorithm performs excellently in terms of robustness and can work stably under Gaussian conditions. However, this algorithm introduces a suboptimal fading factor into the prediction error covariance matrix and enhances reliability and stability by adjusting the gain online. Since the adjustment of the fading factor mainly focuses on the part related to the state transition matrix and is mostly a non-differentiated adjustment of a single sub-fading factor type, the correction of the filtering model and the interpretability of parameter estimation are relatively weak, and the estimation performance still needs to be further improved. In addition, when dealing with non-Gaussian problems, this algorithm assumes that the noise and state variables of the system follow a Gaussian distribution. However, in a non-Gaussian environment, this assumption deviates significantly from the actual distribution, resulting in a decline in estimation performance. Adjusting the relevant terms of the state transition matrix in the prediction error covariance matrix using the fading factor can actually be equivalently described as a dynamic adjustment of the process noise covariance. Therefore, the existing STSCKF algorithm has deficiencies in effectively adjusting the observation noise covariance in real time. Finally, there is a linear approximation error problem in the calculation process of the fading factor in this algorithm, which leads to relatively low filtering accuracy. Summary of the Invention

[0006] Object of the Invention: Aiming at the complex nonlinear non-Gaussian ship motion state model, the existing STSCKF has problems such as insufficient non-Gaussian processing ability, poor model interpretability, suboptimal adjustment of the observation noise covariance, and linear approximation error. The present invention proposes a multi-algorithm fusion USV motion state estimation method based on the coupling of GS-STSCKF and credibility theory.

[0007] Technical Solution: A multi-algorithm fusion USV state estimation method based on the coupling of GS-STSCKF and credibility theory for a nonlinear non-Gaussian system, comprising:

[0008] Input the measurement information of the sensor into the credible GS-STSCKF algorithm to obtain the filtering result;

[0009] At the same time, input the difference between the state estimation value of the filter and the one-step state prediction into the improved BLS network to output the error between the filtering result and the actual value;

[0010] Sum the filtering result output by the credible GS-STSCKF algorithm and the output of the improved BLS network to obtain the estimated state value of the USV;

[0011] Among them, the credible GS-STSCKF algorithm is obtained according to the following steps:

[0012] Improve the existing STSCKF filtering algorithm to obtain the improved STSCKF filtering algorithm. The improvements include: directly applying the fading factor matrix to the adjustment of the covariance matrix of process noise, that is, differentially adjusting each element in the covariance matrix of process noise through the multiple fading factors in the fading factor matrix;

[0013] Improve the existing Gaussian fitting technique to obtain the improved Gaussian fitting technique; use the improved Gaussian fitting technique to enhance the non-Gaussian processing ability of the improved STSCKF filtering algorithm to obtain the GS-STSCKF algorithm;

[0014] Construct a trust factor on the GS-STSCKF algorithm to obtain the credible GS-STSCKF algorithm. Based on the trust factor, the credible GS-STSCKF algorithm obtains the conditions for the nonlinear non-Gaussian filter, and uses these conditions combined with the range of the trust factor value and the HPO-DOA algorithm to adjust the covariance of the observation noise in real time.

[0015] Beneficial effects: Compared with the prior art, the present invention has the following advantages:

[0016] (1) The method of the present invention improves the existing STSCKF algorithm. By using the equivalent dynamic adjustment theory and the Cholesky triangular decomposition idea, the fading factor is directly applied to the process noise covariance matrix, and each element in the process noise covariance is finely differentially adjusted through multiple fading factors, solving the problem that a single fading factor cannot perform a detailed differential adjustment on each element in the covariance; the method of the present invention also proposes two methods for calculating the fading factor matrix: direct matrix solution and matrix trace solution. These improvements not only enhance the model interpretability of the algorithm but also significantly improve its estimation performance;

[0017] (2) To solve the problem of poor adaptability of the existing STSCKF algorithm in a non-Gaussian environment, the method of the present invention designs a GS-STSCKF algorithm with better non-Gaussian processing ability, and uses an improved Gaussian fitting technique based on a Gaussian component fusion strategy to enhance the non-Gaussian processing ability of the STSCKF algorithm, significantly improving the filtering accuracy. This improved Gaussian fitting method adopts a Gaussian component fusion strategy in the reduction control part, which can retain more Gaussian subterms while ensuring the real-time performance of the algorithm, thereby improving the filtering accuracy of the algorithm;

[0018] (3) From the perspective of the consistency problem between the calculated mean square error and the true mean square error, the method of the present invention constructs a trust factor under the GS-STSCKF framework, and based on the calculation problem of the trust factor, derives a condition capable of designing a high-performance non-linear and non-Gaussian filter. By using this condition in combination with the range of the trust factor value and the HPO-DOA algorithm, the real-time adjustment of the observation noise covariance is realized, thereby solving the deficiency of the existing STSCKF algorithm in the adjustment of the observation noise covariance;

[0019] (4) The method of the present invention simultaneously conducts a coupling analysis of the credibility and the strong tracking theory, studies the closed-loop optimization iteration relationship between the two, and then optimizes the initial weights of the BLS through a genetic algorithm based on an improved roulette wheel, and optimizes the regression parameters by using the HPO-DOA algorithm, thereby improving the convergence speed and network accuracy of the BLS;

[0020] (5) The method of the present invention utilizes the non-linear mapping ability and adaptive ability of the improved BLS network to correct the result of the credible GS-STSCKF algorithm from the compensation perspective, so as to optimize and correct the linear approximation error caused by the construction of the pseudo matrix, and further improve the estimation accuracy of the filtering algorithm. Description of the Drawings

[0021] Figure 1 It is a schematic diagram of the STSCKF filtering process;

[0022] Figure 2 It is a flow chart of the GS-STSCKF algorithm;

[0023] Figure 3 It is a flow chart of the HPO-DOA algorithm;

[0024] Figure 4 It is a closed-loop coupling iteration analysis diagram;

[0025] Figure 5 It is a basic structure diagram of the BLS;

[0026] Figure 6 It is a flow chart of the improved GA for optimizing the initial weights of the BLS;

[0027] Figure 7 It is a schematic diagram of the credible GS-STSCKF algorithm based on the improved BLS;

[0028] Figure 8 It is an overall block diagram;

[0029] Figure 9 It is a schematic diagram of the estimation error of the process noise covariance component 1;

[0030] Figure 10 It is a schematic diagram of the estimation error of the process noise covariance component 2;

[0031] Figure 11 Schematic diagram of the estimation error of the observation noise covariance component 1;

[0032] Figure 12 Schematic diagram of the estimation error of the observation noise covariance component 2;

[0033] Figure 13 Schematic diagram of the state estimation error;

[0034] Figure 14 Experimental scenario diagram;

[0035] Figure 15 Schematic diagram of the actual state estimation error. Specific implementation manners

[0036] The technical solution of the present invention will be further elaborated below in conjunction with the accompanying drawings and embodiments.

[0037] Embodiment 1:

[0038] To improve the motion state estimation accuracy of the unmanned ship in a harsh water environment, ensure that the navigation control system can adjust the rotational speed or power output of the motor in real time and correctly, and ensure that the USV can sail according to the predetermined state. This embodiment proposes a multi-algorithm fusion USV state estimation method based on the coupling of GS-STSCKF and credibility theory for non-linear and non-Gaussian systems. In this estimation method, it mainly includes 4 stages:

[0039] Stage 1: By changing the way of adjusting the prediction error covariance matrix with the traditional fading factor, an improved STSCKF filtering algorithm is proposed. Through the equivalent dynamic adjustment theory and Cholesky triangular decomposition idea, the fading factor is directly applied to the process noise covariance matrix, and each element in the process noise covariance is finely and differentially adjusted through multiple fading factors. In addition, two methods for calculating the fading factor matrix are also proposed.

[0040] Stage 2: Enhance the non-Gaussian processing ability of the existing STSCKF algorithm through an improved Gaussian fitting technique. This improved Gaussian fitting method adopts a Gaussian component fusion strategy in the reduction control part. This strategy can retain more Gaussian sub-items while ensuring the real-time performance of the algorithm, thereby improving the filtering accuracy of the algorithm.

[0041] Stage 3: By studying the consistency between the true mean square error and the calculated mean square error, a trust factor under the GS-STSCKF framework was constructed to measure the reliability of the filter. Based on the calculation problem of the trust factor, a condition for designing a high-performance nonlinear and non-Gaussian filter was derived. Using this condition in combination with the range of the trust factor value and the HPO-DOA algorithm, real-time adjustment of the observation noise covariance can be achieved, thus solving the deficiency of the existing STSCKF algorithm in the adjustment of the observation noise covariance. At the same time, a coupling analysis of the credibility and strong tracking theory was carried out to study the closed-loop optimization iteration relationship between the two.

[0042] Stage 4: A credible GS-STSCKF algorithm based on the improved BLS was proposed. This algorithm optimizes the initial weights of the BLS through a genetic algorithm based on the improved roulette wheel and optimizes the regression parameters using the HPO-DOA algorithm, thereby improving the convergence speed and network accuracy of the BLS. Then, using the nonlinear mapping ability and adaptive ability of the improved BLS network, the results of the credible GS-STSCKF algorithm were corrected from the compensation perspective to optimize and correct the linear approximation error caused by the pseudo-matrix construction, further improving the estimation accuracy of the filtering algorithm.

[0043] The following further explains each stage.

[0044] Now, a nonlinear and non-Gaussian system is modeled to obtain the following model of a discrete nonlinear and non-Gaussian system:

[0045] x k+1 =f k (x k )+w k+1,k (1)

[0046] z k+1 =h k (x k )+v k (2)

[0047] where k represents the time, x k ∈R n denotes the system state vector, f k :R n →R n 、h k :R n →R p represent the system state matrix and the measurement matrix respectively, w k+1,k ∈R n denotes the non-Gaussian process noise, z k ∈R p is the observation vector, v k ∈R pdenotes non-Gaussian observation noise, and the state noise and measurement noise are independent of each other.

[0048] The following gives the motion model of the unmanned ship. The state vector x of this unmanned ship motion model 无人船k and the state transition function f(·) are respectively:

[0049]

[0050]

[0051] In the formula, and λ k are respectively the latitude arc length and longitude arc length of the unmanned ship's navigation. and are respectively the east component and north component of the water flow velocity. υ k is the velocity of the unmanned ship relative to the water flow. H k is the course. Ω k is the course change rate. β * is the water flow anti-correlation time, which is related to the course H k β *-1 = 15 n mile / H, and the sampling period T = 1 s.

[0052] In addition, the observation vector z of the unmanned ship motion model 无人船k and the observation matrix G are respectively:

[0053]

[0054] G = I 7×7 (6)

[0055] In the formula, and are the latitude arc length, longitude arc length and speed of the unmanned ship's navigation measured by the global satellite navigation system. and are respectively the course and course change rate of the electric-driven unmanned ship measured by the gyroscope. and are respectively the east component and north component of the water flow velocity measured by the current meter.

[0056] Now, a further explanation is given for Phase 1.

[0057] The core idea of the existing STSCKF filtering algorithm is to introduce a time-varying fading factor to adjust the covariance matrix of the prediction error, so that the filtering process has the ability to sense and cope with the situation of inaccurate models. The filtering process of the existing STSCKF filtering algorithm is as Figure 1As shown, in terms of the adjustment method, when the statistical characteristics of the noise are distorted, adjusting the process noise variance is more intuitive and easier to understand than the existing adjustment methods. Therefore, next, considering from the concept of orthogonality, the purpose of indirectly adjusting the one-step prediction covariance matrix and gain is achieved by allowing the fading factor to adjust the covariance of the process noise. At the same time, if the covariance of the process noise is in the form of a matrix, the ability to perform differential fading adjustment for different data channels also needs to be realized. Therefore, the calculation formula of the one-step prediction covariance in Figure 1 is modified to:

[0058]

[0059] where, is the original fading factor matrix L k obtained through the Cholesky triangular decomposition technique, that is:

[0060]

[0061] where,

[0062]

[0063] is called the fading matrix and contains n different fading factors. Finally, the calculated is:

[0064]

[0065] In this embodiment, the fading factor matrix is calculated by constructing a pseudo matrix. The pseudo observation matrix based on the SCKF is:

[0066] H k =(P xz,k|k-1 ) T (P k|k-1 ) -1 (11)

[0067] The pseudo state transition matrix is:

[0068] F k|k-1 =N k (M k-1 ) -1 (12)

[0069] where,

[0070]

[0071] M k-1 =chol(P k-1|k-1 ) (15)

[0072] Based on this, the complex calculation problem of the non - linear fading factor is simplified to the calculation problem of the linear fading factor.

[0073] At this time, the recursion of the filter is simplified to:

[0074]

[0075] Combining formula (17) and formula (19), the linearized one - step prediction covariance matrix of STSCKF can be obtained as:

[0076]

[0077] Another representation form of the orthogonality principle is known as:

[0078]

[0079] Among them, V k represents the innovation covariance matrix.

[0080] Thus, substituting formula (18) into formula (22) gives:

[0081]

[0082] Substituting formula (21) into formula (23) again gives:

[0083]

[0084] Thus, in this embodiment, based on formula (24), two different solving methods are designed, which are respectively called the direct matrix solving algorithm and the matrix trace solving algorithm.

[0085] Among them, the specific operations of the direct matrix solving algorithm include:

[0086] Since Q k,k-1 is a positive definite symmetric matrix, according to the Cholesky triangular decomposition idea, it can be decomposed into:

[0087]

[0088] Substituting formula (25) into formula (24) becomes:

[0089]

[0090] Let:

[0091]

[0092] Then, formula (26) becomes:

[0093]

[0094] The matrix A can be obtained from formula (28). k , and then the pseudo-inverse matrix of H k can be obtained by using the pseudo-inverse technology. is:

[0095]

[0096] Finally, the decomposed fading factor matrix can be obtained as:

[0097]

[0098] Assume that in general, when the dimensions of the observation vector and the state vector are different, i.e., p≠n and p < n, the matrix A k is a p×n matrix and is no longer a square matrix, while S k is a p×p matrix. Therefore, the solution of the matrix A k is not unique. The method for solving the matrix A k adopted here is:

[0099] First, decompose S k into the product of a p×p matrix B k and its transpose through Cholesky decomposition, i.e.:

[0100]

[0101] Next, add all-zero columns after the p-dimensional square matrix B k to make its number of columns up to n columns, and the new matrix obtained is made into the matrix A k , which can satisfy the expression

[0102] It should be noted that the A k here is only one of the solutions that can be obtained, a special solution among many solutions, and may not necessarily be the optimal solution. This may be the possible reason why the estimation accuracy of the strong tracking filter is affected to a certain extent under this multiple-order fading factor calculation method.

[0103] Next, another method for solving the multiple-order fading factor is proposed.

[0104] Among them, the specific operations of the matrix trace solution algorithm include:

[0105] The pseudo-inverse matrix of H k and can be obtained by using the pseudo-inverse technology and where is calculated by formula (29), and the calculation formula is as follows:

[0106]

[0107] Thus, formula (24) becomes:

[0108]

[0109] Taking the trace of both sides of formula (33) gives:

[0110]

[0111] Let:

[0112]

[0113] From this, formula (34) can be simplified to:

[0114]

[0115] Substituting formula (10) into formula (36) gives:

[0116]

[0117] According to formula (37), we can obtain:

[0118]

[0119] where, represents the element in the \(i\)-th row and \(i\)-th column of the process noise covariance matrix \(Q\) k,k-1 , and represents the element in the \(i\)-th row and \(i\)-th column of \(C\) k . Thus, the fading factor matrices \(L\) k and before and after decomposition can be obtained.

[0120] The existing STSCKF algorithm performs excellently in terms of robustness and can work stably under Gaussian conditions. However, this algorithm assumes that both the noise and state variables of the system follow a Gaussian distribution, which has a large deviation from the actual situation in a non-Gaussian environment, resulting in a decline in estimation performance. To solve this problem, in this embodiment, Gaussian fitting technology is used to propose a GS-STSCKF algorithm to enhance the ability to process non-Gaussian noise. In the Gaussian sum filtering algorithm, the process of state estimation not only requires solving the state mean and variance but also calculating parameters such as the weights corresponding to the Gaussian subterms. The process of the GS-STSCKF algorithm is described in detail in 5 steps, including:

[0121] Step 1: Initialization. Set the initial time point \(k = 0\), define the initial state \(p(x_0|z\) -1 ) = \(p(x_0)\), and use it as \(N\)k|k-1 Summation of Gaussian sub - terms.

[0122] Using the EM algorithm, the initial probability density function \(p(x_0)\), the process noise probability density function \(p(w\) k ) and the measurement noise probability density function \(p(v\) k ) are all expressed in the form of Gaussian sum as follows:

[0123]

[0124] Where, and are the weights of their corresponding Gaussian sub - terms, and there is

[0125] Step 2: Distributed filtering. The filtering probability density function is approximated in the form of Gaussian sum as in formula (42):

[0126]

[0127] Where the measurement and: \(N\) k|k =\(N\) k|k-1 ·\(r\) k . In addition, based on the predicted state mean the predicted covariance matrix and the measurement noise mean and the measurement variance matrix the STSCKF algorithm is used to calculate the filtering mean and covariance matrix of the \(i\) - th Gaussian sub - term

[0128] At this time, the measurement likelihood function for the state estimation of the \(i\) - th Gaussian sub - term can be expressed as:

[0129]

[0130] The calculation methods of indices \(j\) and \(l\) are:

[0131]

[0132] Where \(i = 1,2,\cdots,N\) k|k ; \(j = 1,2,\cdots,N\) k|k-1 ; \(l = j = 1,2,\cdots,r\) k ; The symbol represents the floor operation. The filtering weight corresponding to the Gaussian sub - term needs to synthesize three items of information, namely the predicted filtering weight the weight occupied by the measurement noise probability density function and the likelihood function of the state estimation Therefore The calculation method of

[0133]

[0134] Step 3: Global point estimation. After each Gaussian sub-term obtains the filtering state at time k, the global filtering mean at time k can be calculated using formulas (46) and (47) and covariance matrix P k|k .

[0135]

[0136] Similarly, the state estimates at other time points can also be calculated from formulas (46) to (47).

[0137] Step 4: Reduction control. Usually, the process noise and measurement noise distributed in the form of Gaussian sum will cause the continuous increase of the number of Gaussian sub-terms over time, such as N in formula (42) k|k and N given later k+1|k . This will inevitably greatly increase the computational cost and reduce the practicability of the filtering. Therefore, to improve the real-time performance of the algorithm and ensure the filtering accuracy at the same time, this embodiment adopts a Gaussian component fusion strategy. After calculating the estimated mean and covariance of each Gaussian sub-term at time k, first only retain all Gaussian components with weights greater than G′.

[0138]

[0139] where G′ is the weight of the Gth Gaussian component after sorting the weights from largest to smallest.

[0140] Among the retained Gaussian components, find the component with the largest weight:

[0141]

[0142] and find all eligible Gaussian components according to the following criteria:

[0143]

[0144] where

[0145] These Gaussian components are combined into a Gaussian distribution by weighted averaging, and its mean and covariance are:

[0146]

[0147] Delete all Gaussian components that have undergone the fusion operation, and then repeat the above steps until all Gaussian components are completed. Finally, only retain the top S sub-Gaussian distributions with relatively large weights, and renormalize the corresponding weights to reduce the computational complexity.

[0148] Step 5: Prediction update. The predicted state probability density function can be expressed as follows:

[0149]

[0150] where

[0151]

[0152] It can be seen that the predicted weight corresponding to the Gaussian sub-term combines two pieces of information, namely the filtering weight and the weight occupied by the process noise probability density function In addition, based on the estimated state mean the estimated variance matrix the process noise mean the process noise covariance matrix Use the STSCKF algorithm to calculate the predicted mean and covariance matrix of the i-th Gaussian sub-term

[0153] In addition, the calculation methods of the indicators c and d are as follows:

[0154]

[0155] where i = 1, 2,..., N k+1|k ; c = 1, 2,..., N k|k ; d = 1, 2,..., q k .

[0156] So far, the state filtering process at time k has been completed. Set the time point to update k = k + 1, and jump to step 2 to achieve step-by-step state filtering. The schematic diagram of the GS-STSCKF filtering algorithm is as Figure 2 shown.

[0157] In step 4 above, the Gaussian component fusion strategy of the robust Gaussian sum ensemble Kalman filtering algorithm is added to optimize and improve the GS-STSCKF.

[0158] Under normal circumstances, Gaussian noise, process noise, and measurement noise with a formal distribution gradually accumulate over time, resulting in an increasing number of Gaussian subterms. This inevitably increases the computational overhead and reduces the practicality of the filter. At the same time, in the existing GS-SCKF algorithm, the control of the number of Gaussian subterms is mainly achieved by restricting the maximum number of Gaussian subterms, that is, only the first T (T < G) sub-Gaussian distributions with relatively large weights are retained. Although this method can improve the real-time performance of the algorithm, it will cause some effective Gaussian subterms to be discarded, thus significantly reducing the accuracy of the filter. In order to improve the real-time performance of the algorithm while ensuring the filtering accuracy, in this embodiment, Gaussian components that meet the fusion conditions are merged into a Gaussian distribution by weighted averaging. Compared with the existing reduction control method of GS-SCK, this method can retain more Gaussian subterms to improve the filtering accuracy of the algorithm, and at the same time ensure the real-time performance of the algorithm. The above algorithm uses the STSCKF filter to estimate the system state corresponding to each Gaussian subterm for each sub-filter respectively, and then combines the weights corresponding to each subterm to achieve global estimation. Thus, the non-Gaussian processing ability of the existing STSCKF algorithm is enhanced.

[0159] The third stage will be further described below.

[0160] By using a fading factor to adjust the terms related to the state transition matrix in the prediction error covariance matrix, it can actually be equivalently described as a dynamic adjustment of the process noise covariance. Therefore, the above-mentioned proposed GS-STSCKF algorithm has deficiencies in effectively adjusting the observation noise covariance in real time. To address this problem, in this embodiment, from the perspective of the consistency between the calculated mean square error and the true mean square error, a new credibility theory for the GSSTSCKF framework is proposed, and a new trust factor discrimination condition is added in the application of this theory, so as to achieve accurate real-time adjustment of the observation noise covariance. At the same time, this embodiment conducts a coupling analysis of the credibility theory and the strong tracking theory, and studies the closed-loop optimization iteration relationship between the two. Similar to the linear Gaussian Kalman filtering algorithm, for the GS-STSCKF filtering algorithm, if relatively accurate observation noise covariance can be obtained in each Gaussian subterm, the accuracy of the filtering result can be effectively improved.

[0161] Since it is known that the GS-STSCKF algorithm fails to effectively adjust the observation noise covariance in real time, the accuracy of the observation noise covariance is usually poor in each Gaussian subterm. That is, when there is a mismatch in the observation noise covariance in the subsystem, for each Gaussian subterm, there is:

[0162]

[0163] In the formula, represents the true observation noise covariance of the i-th Gaussian subterm, Represents the observation noise covariance of the $i$-th Gaussian sub-term hypothesis in the actual application. Represents the deviation of the $i$-th Gaussian sub-term.

[0164] As can be seen from Equation (16) and Equation (20), if there is a mismatched observation noise covariance sub-term, the filtering iteration formula becomes:

[0165]

[0166] where the superscript $f$ represents the actual calculated value of the sub-filter. However, is not the TMSE of the $i$-th Gaussian sub-term filtering estimate value at time $k$. Because:

[0167]

[0168] From Equation (19) and Equation (20), the TMSE of the filtering estimate value can be obtained as:

[0169]

[0170] where,

[0171]

[0172] where the superscript $m$ represents the true value of the filter. In addition, the calculation methods of the indices $e$ and $g$ are:

[0173]

[0174] For the Kalman sub-filter with inaccurate observation noise covariance in the Gaussian sub-term: 1) If is always a positive definite matrix, then is a positive definite matrix. 2) If is always a negative definite matrix, then is a negative definite matrix.

[0175] When the observation noise covariance sub-term is inaccurate, the consistency between the FMSE and TMSE of each Gaussian sub-term is destroyed. Therefore, the definition of the non-linear non-Gaussian trust factor is realized through the TMSE and FMSE of each sub-term to measure the trust degree of the non-linear non-Gaussian system. Similar to the linear Gaussian system, the TMSE of each Gaussian sub-term in the non-linear non-Gaussian system cannot be obtained during the real-time filtering process, so the calculation of the trust factor first needs to estimate the TMSE.

[0176] If an inaccurate observation noise covariance is used in the sub-filter, the true estimated error covariance matrix of each Gaussian sub-term cannot be obtained, and the final estimated result is not optimal. And the non-linear non-Gaussian filter studied in this embodiment is split into multiple linear Gaussian sub-filters by using the Gaussian sum algorithm and constructing a pseudo matrix, and thus the trust factor under the GS-STCKF framework can be constructed:

[0177]

[0178] where is the 2-norm of and the value range of is (0, +∞), so formula (65) normalizes it. t(k) is used as the trust factor, and its value range is 0 to 1, which can be used to measure the closeness of the FMSE and TMSE of each sub-filter. The closer t(k) is to 1, the closer the FMSE and TMSE of each sub-filter are, and thus the higher the credibility of the GS-STCKF filter model. When the FMSE and TMSE of each sub-filter are exactly equal, it indicates that the noise covariances of each sub-filter are matched. At the same time, t(k) is affected by , and when Tr

[0179] is larger, t(k) is smaller. It is known that in the actual filtering process, the of each Gaussian sub-term cannot be directly obtained. Therefore, if the trust factor is to be estimated according to formula (65), the estimation problem of

[0180] needs to be discussed.

[0181] The innovation of the known sub-filter is:

[0182]

[0183] The covariance matrix of

[0184]

[0185] is: The superscript f represents the calculated value. However is not the true value of the covariance matrix of

[0186]

[0187] The true covariance is: The superscript m represents the true value. From formula (67) and formula (68), we can get:

[0188]

[0189] For the Kalman sub-filter with inaccurate observation noise covariance in the Gaussian sub-term: 1) If is always a positive definite matrix, then is a positive definite matrix. 2) If is always a negative definite matrix, then is a negative definite matrix.

[0190] When there is a mismatched observation noise covariance, using formulas (58) and (62), we can obtain:

[0191]

[0192] According to equations (69) and (62), we can get:

[0193]

[0194] where and can both be calculated or estimated.

[0195] Sage-Husa adaptive Kalman filtering is a method for estimating the observation noise covariance using the covariance matching method. Using this method to estimate the observation noise covariance, this method can be briefly described as:

[0196]

[0197] where can be obtained from the time-varying noise statistic estimator:

[0198]

[0199] In the formula, d k =(1 - b) / (1 - b k+1 ), is the forgetting factor.

[0200] In addition, the calculation methods of the indicators h and o are:

[0201]

[0202] According to equation (72), we can get:

[0203]

[0204] Therefore, according to equation (66), the estimated value of t(k) can be obtained

[0205] From equation (77), we can get:

[0206]

[0207] If and only if the formula when And when when Therefore, directly using this algorithm easily causes the filter to diverge.

[0208] According to Equation (77), it can be known that when when

[0209]

[0210] Among them, represents the observation noise covariance satisfied by the Gaussian sub-term. It can be seen from the above Equation (83) that the Sage-Husa adaptive Kalman filter algorithm is at when converges to That is is the convergence estimate value of this algorithm. To sum up, by adjusting to make it satisfy:

[0211]

[0212] Among them, s.t. represents the constraint condition, and the calculation of index l is shown in Equation (44). ensures the same estimation effect as the SageHusa adaptive Kalman filter algorithm, and the constraint condition ensures that there is no problem of divergence of the observation noise covariance in the estimation process. Thus, in each Gaussian sub-term, finding the observation noise covariance sub-term that satisfies Equation (85) can improve the performance of the corresponding sub-filter. Therefore, for the studied nonlinear non-Gaussian system, it is known from the previous analysis that it can be superimposed and fitted by N k|k Gaussian linear sub-terms. Therefore, according to Equation (85), the conditions for designing a high-performance filter for a nonlinear non-Gaussian system are:

[0213]

[0214] The combination of the Hunter-prey optimizer (HPO) and the Dingo Optimization Algorithm (DOA) is used to realize the simultaneous estimation of multiple observation covariance sub-terms, so as to obtain the required This algorithm combines the "hunter-prey discrimination strategy" of the HPO algorithm and the "diversified predation strategy" of the DOA algorithm to improve the probability of updating the actual optimal value in a limited number of iterations and obtain better global optimization capabilities. In the HPO algorithm, an individual is selected from the population as the prey according to a certain standard, and the other individuals are all hunters. The prey "escapes" to the optimal individual of the previous iteration, and the other individuals approach the "prey" according to the same principle. By distinguishing between "hunters" and "prey", and having the prey approach the safest location (i.e., the optimal position in the previous iteration), and then the hunters tracking the prey, the HPO algorithm achieves the purpose of updating and iterating for optimization. This algorithm uses the "prey" as the optimization intermediary. In each iteration, the selected prey "escapes" to the most optimal individual of the previous iteration, and the other individuals "pursue" the prey as hunters, effectively avoiding local optimization. However, compared with the DOA algorithm, the method of hunters tracking prey is relatively single. Considering the time cost, the probability of updating the actual optimal value in a limited number of iterations becomes lower. In the DOA algorithm, the wild dogs play the role of "hunters" in the hunter-prey algorithm. This algorithm classifies wild dogs according to their hunting methods, and the hunting behaviors are divided into "collective attack", "persecution", and "scavenging". At the same time, the "survival rate" mechanism is used to replace the inferior individuals. This algorithm approaches the "optimal individual of the previous generation" with the above-mentioned hunting behaviors, and it can be considered that the prey is the "optimal individual of the previous generation". The DOA algorithm simulates the predation behavior of a wild dog population, effectively increasing the probability of searching for the optimal value in a limited number of iterations. However, in essence, this algorithm directly uses the optimal value of the previous generation of iterative individuals as the prey, and all other individuals directly attack and prey on it, which is likely to cause the situation of local optimum. Therefore, by combining the advantages of the above two algorithms, the combination of "distinguishing hunters and prey" and "diversified predation behaviors" is carried out. The specific improvement method is:

[0215] (1) Apply the hunter-prey discrimination strategy in the HPO algorithm to change the "prey" of the DOA algorithm from the "optimal value of the previous iteration" to the individual acting as the "prey" found by the hunter-prey algorithm. Rely on the method of "hunters chasing prey and prey fleeing to a safe point" to obtain better global optimization capabilities, effectively avoiding the local optimum situation caused by directly using the optimal value of the previous generation of iterative individuals as the prey in the DOA algorithm.

[0216] (2) Replace the single predation method in the original HPO algorithm with the diversified predation strategy of the DOA algorithm, so as to effectively increase the probability of searching for the optimal value in a limited number of iterations.

[0217] Let The order be r. In an r k ×r-dimensional space, randomly generate a population X = (x1, x2,..., x n)。The \(i\) -th individual corresponds to a vector indicating its position in the k \(r\times r\) -dimensional space. Calculate the fitness value corresponding to each individual according to the fitness function.

[0218] STEP1 Generation of the initial population. The position of each member in the initial population is randomly generated in the search space by Equation (86). At the same time, the individuals estimated by the previous EM algorithm are put into the randomly generated initial population.

[0219] x i \(=\text{rand}(d, 1)\times(\text{ub}-\text{lb})+\text{lb}\ (86)\)

[0220] where \(x\) i is the position of the hunter or prey, \(\text{lb}\) is the minimum value (lower bound) of the problem variable, \(\text{ub}\) is the maximum value (upper bound) of the problem variable, and \(d\) is the number of problem variables (dimensions).

[0221] STEP2 Calculate the fitness value of each individual. The fitness function is:

[0222]

[0223] STEP3 Distinguish hunters and prey. According to the hunter - prey optimization algorithm, the judgment criterion for distinguishing whether an individual is a hunter or a prey each time is that the individual with the largest distance from the average position is regarded as the prey (\(P\) pos ):

[0224]

[0225] where the average position is The distance between each individual and the average position is \(D\) i \(=\vert x\) i \(-\mu\vert\). Otherwise, it will be regarded as a hunter.

[0226] STEP4 Update the position of the prey. According to the hunter - prey optimization algorithm, use the following formula (89) to update the position of the prey:

[0227] x i \((t + 1)=x^*(t)+C Z\cos(2\pi R_4)(x^*(t)-x\) i \((t))\ (89)\)

[0228] where \(x\) i \((t)\) is the current position of the prey; \(x\) i \((t + 1)\) is the next iteration position of the prey; \(x^*(t)\) is the global optimal position; \(P\) is the adaptive parameter calculated by Equation (90); \(R_4\) is a random number in the range \([-1, 1]\); \(Z\) is the balance parameter between exploration and exploitation, whose value decreases during the iteration process of the algorithm and is calculated by Equation (91).

[0229] P = R1 < C; IDX = (P == 0)? 90 :

[0230]

[0231] R1 and R3 are random vectors within [0, 1], P is the index value where R1 < C, R2 is a random number within [0, 1], IDX is the index value of vector R1 that satisfies the condition (P == 0), C is the balance parameter between exploration and exploitation, whose value decreases from 1 to 0.02 during the iteration process, and is calculated as follows:

[0232] C = 1 - it * (0.98 / MaxIt)

[0233] where it is the current iteration number and MaxIt is the maximum number of iterations.

[0234] STEP5 Update of the hunter (dingo) position.

[0235] Step 5.1 Strategy 1: Group attack Predators usually employ highly intelligent hunting techniques, while dingoes tend to hunt small prey alone, such as rabbits. However, when facing large prey like kangaroos, they quickly form groups and exhibit collective behavior of cooperative hunting. Dingoes have the ability to find prey and surround it skillfully, and their behavior can be represented by the following formula (93):

[0236]

[0237] where, x i (t + 1) is the new position of an individual (representing the movement of the dingo); na is a random integer generated in the reverse order of [2, SizePop / 2], where SizePop is the size of the population; is the group of all dingoes that will attack, where X is the randomly generated dingo population; x i (t) is the current position of the individual; β1 is a random number uniformly generated within [-2, 2], which is a scaling factor.

[0238] Step 5.2 Strategy 2: Persecution

[0239] Dingoes usually hunt small prey until they catch it alone. The following formula simulates this behavior:

[0240]

[0241] where the value of β1 is the same as that in formula (93), β2 is a random number uniformly generated within the interval [-1, 1], r1 is a random number generated within the interval [1, SizePop], x r1(t) is the r1-th individual randomly selected, where i ≠ r1.

[0242] Step 5.3 Strategy Three: Scavenging

[0243] The behavior of scavengers can be described as when dingoes wander in their natural habitat and accidentally find carrion and consume it. This behavior is simulated using the following equation (95):

[0244]

[0245] where β2 has the same value as in equation (94), and σ is selected as follows: generate a random number in [0, 1], if it is less than or equal to 0.5, return 0, otherwise return 1.

[0246] Step 5.4 Strategy Four: Survival rate of dingoes

[0247] Dingoes in Australia are facing the threat of extinction, mainly due to illegal hunting. In the DOA algorithm, the survival rate of dingoes can be given by the following equation:

[0248]

[0249] where, fitness max and fitness min are the best and worst fitness values in the current generation respectively, and fitness(i) is the current fitness value of the i-th individual. Equation (96) is applied to low survival rates through an algorithm (if the survival rate is less than or equal to 0.3, the current low survival rate search agent is updated using equation (97)), for example, the survival rate value is equal to or less than 0.3.

[0250]

[0251] where, x i (t) is the individual with a lower survival rate to be updated, r1 and r2 are random numbers generated in the interval [1, SizePop], r1 ≠ r2, x r1 (t) and x r2 (t) are the r1-th and r2-th individuals randomly selected.

[0252] The process of the HPO-DOA algorithm is as follows:

[0253] 1) Randomly generate population individuals in the defined interval [x min , x max , and at the same time put the individuals estimated by the previous EM algorithm into the randomly generated initial population.

[0254] 2) Construct the fitness function Substitute the population individuals as the observation noise covariance corresponding to each Gaussian sub - term into the filtering algorithm, and obtain the corresponding by equations (68) and (69), and calculate the fitness value.

[0255] 3) Find the global optimal position according to each fitness value.

[0256] 4) Distinguish hunters and prey among the population individuals.

[0257] 5) Update the individuals in the population according to equations (88)-(96).

[0258] 6) Calculate the fitness value of the updated individuals and find the global optimal position.

[0259] 7) Judge whether the number of iterations reaches the upper limit. If yes, output the corresponding optimal individual as the estimated value of the observation noise covariance of each Gaussian sub - term so as to obtain an accurate non - Gaussian observation noise estimate; otherwise, go to step 4).

[0260] 8) Calculate t(k) according to equation (65).

[0261] 9) Judge whether t(k) is greater than 0.9. If yes, output to GS - STSCKF; otherwise, put into the initial population for further optimization.

[0262] The flowchart of this algorithm is as Figure 3 shown.

[0263] In the credibility theory of this embodiment, since the true mean - square error cannot be obtained in the calculation process of t(k) Therefore, a condition for designing a high - performance filter is extended, so as to estimate the value of the true observation noise covariance sub - term value and then t(k) can be further calculated. However, in practical applications, the observation noise covariance matrix estimated by this extended credibility theory is used as the estimated value and substituted into the GS - STSCKF filter. Therefore, when t(k) is used as the discriminant condition, is used to solve instead of Therefore, in practical applications, the calculation process of t(k) has to be considered from another perspective.

[0264] It is known that in STSCKF, the estimated innovation covariance matrix is:

[0265]

[0266] where, and

[0267]

[0268] Thus, estimate the innovation covariance matrix S zz,k|k-1 The difference between the true value and the calculated value of is:

[0269]

[0270] wherein, the solution process of S is described by a statistical approximation method. Therefore, when zz,k|k-1 is more accurate, is closer to 1. Thus, when is closer to the true value,

[0271] The above analysis completes the calculation when the trust factor t(k) is used as a discriminant condition in practical applications. In the application of the existing credibility theory, only the conditions for designing a high-performance filter derived from the calculation process of the trust factor are used as the fitness function for optimization. However, in the algorithm flow for estimating the noise covariance, the value and contribution shown by the trust factor are not fully reflected. Therefore, in this embodiment, step 9) is added to the algorithm flow, and whether the trust factor value meets the requirements is used as a discriminant condition, thereby further improving the practical application effect of the trust theory.

[0272] Closed-loop Coupled Iterative Optimization Analysis of Credibility and Strong Tracking Theory

[0273] From Figure 4 it can be seen that when is optimized by the fading factor matrix to be more accurate, according to Equation (68), the calculated covariance matrix of the filtering calculation of is related to Therefore also becomes more accurate, thereby improving the optimization effect of the fitness function in Equation (86), making the estimated also more accurate. Then, according to Equation (24), the calculation of the multiple fading factor matrix depends on the observation noise covariance Therefore, accurate can improve the adjustment performance of further enhancing the accuracy of after the fading factor is optimized Thus, a closed-loop coupled iterative optimization method combining credibility and strong tracking theory is achieved.

[0274] ​This method of closed-loop coupling iterative optimization enables the system to continuously self-adjust and optimize. Through self-optimization, the system can maintain high performance in complex environments and possess stronger adaptability and robustness. Specifically, higher computational accuracy, better regulation performance, and continuously optimized system parameters together improve the overall reliability and stability of the system, reducing possible errors and uncertainties.

[0275] Now, a further explanation of Phase Four will be given.

[0276] Since the calculation of the fading factor and the design and application of the credibility theory involve the construction of pseudo matrices, this inevitably introduces a certain degree of linear approximation error. To further optimize and correct these errors, this embodiment proposes a width learning neural network strategy based on an improved genetic algorithm and the HPO-DOA algorithm. This strategy aims to correct the output of the credible GS-STCKF algorithm from a compensation perspective, thereby significantly improving its estimation accuracy.

[0277] The BLS system is established in the form of a planar network. First, the input data is converted into mapped feature nodes through a feature mapping function. Secondly, the mapped feature nodes are enhanced into enhanced nodes through an activation function for width structure expansion. Finally, all feature nodes and enhanced nodes are combined into an extended matrix and connected to the output node. BLS only needs to use the ridge regression generalized inverse to calculate the output connection weights of the model, greatly reducing the training time of the model. In addition, when the system model encounters new input samples, the incremental learning method is used to expand the system model, and the system model can be quickly reconstructed without complete retraining. The basic structure of the BLS model is as Figure 5 shown.

[0278] Denote the sample feature data set as X ∈ R N×P , the sample label encoding as Y ∈ R N×Q , N is the number of samples, P is the number of sample features, and Q is the total number of sample categories. First, s groups of mapped features are obtained:

[0279] Z i =φ i (XW ei +β ei ), i = 1, 2,..., s (101)

[0280] where, W ei is the weight value of a group of mapped features, e is the number of nodes of a group of mapped nodes, the weight W ei and the bias β ei follow the Gaussian distribution within the range of [0, 1], φ i (·) is a linear function, and a group of mapped features Z i ∈R N×e is obtained. To overcome the weight Wei For the random characteristics, use a sparse autoencoder to fine-tune the random features to make them a set of sparse and compact features, effectively reducing the linear correlation degree of the feature nodes. After cycling s times, all the mapped feature sets Z s =[Z1, Z2,..., Z s are obtained, and the dimension of Z s ∈R N×(e×s) .

[0281] Secondly, perform non-linear enhancement on the mapped feature set Z s . The activation method of each enhanced node group containing f nodes is as follows:

[0282] H j =ξ j (Z s W hi +β hi ), j = 1, 2,..., t (102)

[0283] where t is the number of enhanced node groups, W hi is a random matrix after orthogonal normalization, serving as the weight value of a set of enhanced features, and ξ j is the sigmoid activation function. All the enhanced feature sets H t =[H1, H2,…, H t are obtained, and the dimension of H t ∈R N×(f×t) .

[0284] Finally, concatenate all the mapped feature sets Z s and the enhanced feature sets H t to form an extended feature matrix M = [Z s |H t as the input of the output layer. Then the output of BLS is:

[0285] Y = MW out (103)

[0286] where W out ∈R N×(e×s+f×t) is the connection weight of the output layer.

[0287] Use the ridge regression generalized inverse optimization method to solve for the output weight W out , and the model is constructed.

[0288] W out =(λI + M T M) -1 M T Y (104)

[0289] In this embodiment, an improved genetic algorithm is used to find the global optimal solution to optimize the initial weights of BLS, so as to improve the network accuracy and convergence speed. The genetic algorithm optimizes the BP neural network algorithm to optimize the initial weights in the BLS network, and the determined fitness function is:

[0290]

[0291] where T i represents the actual output value of the i-th training sample, and Y i represents the expected output value of the i-th training sample.

[0292] It is known that when the genetic algorithm optimizes the BLS algorithm, the roulette wheel method is usually selected. Since during the population evolution, generally, there will be some individuals with extremely abnormal fitness values, and these individuals are very likely to determine the selection process, thus reducing the population diversity. Therefore, this embodiment proposes a new roulette wheel selection method, that is, the selected individuals are removed from the total selection sequence each time and no longer participate in the next selection process. The specific selection steps are as follows:

[0293] 1) Arrange the individuals according to the magnitude of their fitness values, sum up all the obtained fitness values, and denote the sum as S.

[0294] 2) Generate a random number M, M ∈ (0, S).

[0295] 3) Starting from the fitness function value of the first individual, successively sum it with the fitness function values obtained by the subsequent individuals. Stop if the cumulative value exceeds M. The last individual whose fitness function value is added is the selected parent.

[0296] 4) Separate these selected individuals, and repeat steps 2) and 3) until enough parents are selected.

[0297] This improved roulette wheel selection method can increase the diversity of individuals because each individual can only be selected once, avoiding the problem that some individuals with abnormal fitness values are over-selected during the selection process. This helps to improve the exploration performance of the algorithm, making the genetic algorithm easier to jump out of the local optimal solution, and thus better search for the global optimal solution. The finally designed process of optimizing the BLS neural network weights by the improved genetic algorithm is as Figure 6 shown.

[0298] For the selection of hyperparameters of BLS, where the regression parameter λ is an adjustable value. If ridge regression is used, usually λ is set to be as close to zero as possible. However, for lasso regression, it needs to be weighed according to the actual scenario, because in practice, when different values of λ are used, the accuracy of the model will also be different. If a value of λ that can make the model accuracy as high as possible can be found, the overall performance of the network can be improved. Therefore, this problem can be regarded as an objective optimization problem.

[0299] Let Y p be the predicted output, Y be the actual value, N be the number of samples, and Y p can be obtained from the actual input and weights:

[0300] Y p = M(λI + M T M) -1 M T M (106)

[0301] In order to minimize the model error, an objective function regarding the regression parameter λ can be defined:

[0302]

[0303] For formula (107), in this embodiment, the HPO-DOA optimization algorithm is used for solving. The specific algorithm process is as follows:

[0304] 1) Randomly generate population individuals in the defined interval [x min , x max .

[0305] 2) Construct a fitness function and calculate the fitness value.

[0306] 3) Find the global optimal position according to each fitness value.

[0307] 4) Distinguish hunters and prey among the population individuals.

[0308] 5) Update the individuals in the population according to equations (88)-(96).

[0309] 6) Calculate the fitness value of the updated individuals and find the global optimal position.

[0310] 7) Judge whether the number of iterations reaches the upper limit. If yes, output the corresponding optimal λ value; otherwise, go to step 4).

[0311] In this embodiment, the improved BLS is used to optimize the credible GS-STSCKF algorithm, and the parameters affecting the pose estimation error (the difference between the state prediction value and the state estimation value As the input of the network, the nonlinear mapping ability and adaptive ability of the network are used to correct the results of the reliable GS-STSCKF algorithm from the compensation perspective, thereby improving the filtering accuracy.

[0312] The specific training method is as follows:

[0313] 1) The difference between the one-step state prediction and the state estimation value of the reliable GS-STSCKF algorithm is used as the input sample of the improved BLS.

[0314] 2) The difference between the true value and the state estimation value is used as the output sample of the network.

[0315] 3) The improved BLS learns the mapping relationship between the prediction error and the actual error of the reliable GS-STSCKF algorithm.

[0316] 4) Output the error between the filtered estimation value and the actual value.

[0317] The flow of the reliable GS-STSCKF algorithm based on the improved BLS is as Figure 7 shown.

[0318] As Figure 7 can be seen, the measurement information of the sensor is input into the reliable GS-STSCKF to obtain the filtering result. At the same time, the difference between the state estimation value and the one-step state prediction of the filtering is input into the trained improved BLS. Then, the network gives the error between the filtering result and the actual value. Finally, the sum of the optimal estimation of the reliable GS-STSCKF and the output of the improved BLS is the optimal estimation value of the reliable GS-STSCKF algorithm based on the improved BLS:

[0319]

[0320] In summary, to improve the motion state estimation accuracy of the unmanned ship in a harsh water environment, ensure that the navigation control system can adjust the rotational speed or power output of the motor in real time and correctly, and ensure that the USV can sail according to the predetermined state. This embodiment proposes a multi-algorithm fusion USV state estimation method based on the coupling of GS-STSCKF and credibility theory for nonlinear non-Gaussian systems. It mainly includes 4 new works.

[0321] In the first new work, an improved STSCKF filtering algorithm is proposed. This algorithm changes the traditional way of adjusting the prediction error covariance matrix by the fading factor. Through the equivalent dynamic adjustment theory and the Cholesky triangular decomposition idea, the fading factor is directly applied to the process noise covariance matrix, and each element in the process noise covariance is finely and differentially adjusted by multiple fading factors. In addition, two methods for calculating the fading factor matrix are also proposed. These improvements not only enhance the model interpretability of the algorithm but also significantly improve its estimation performance.

[0322] In the second new work, a GS-STSCKF algorithm is proposed. This algorithm enhances the non-Gaussian processing ability of the existing STSCKF algorithm through an improved Gaussian fitting technique. This improved Gaussian fitting method adopts a Gaussian component fusion strategy in the reduction control part, which can retain more Gaussian subterms while ensuring the real-time performance of the algorithm, thereby improving the filtering accuracy of the algorithm.

[0323] In the third new work, a reliable GS-STSCKF algorithm is proposed. By studying the consistency problem between the true mean square error and the calculated mean square error, this algorithm constructs a trust factor under the GS-STSCKF framework to measure the reliability of the filter. Based on the calculation problem of the trust factor, a condition for designing a high-performance nonlinear non-Gaussian filter is derived. Using this condition in combination with the range of the trust factor value and the HPO-DOA algorithm, real-time adjustment of the observation noise covariance can be achieved, thus solving the deficiency of the existing STSCKF algorithm in the adjustment of the observation noise covariance. At the same time, a coupling analysis of the credibility and strong tracking theory is carried out to study their closed-loop optimization iteration relationship.

[0324] In the fourth new work, a reliable GS-STSCKF algorithm based on the improved BLS is proposed. This algorithm optimizes the initial weights of the BLS through a genetic algorithm based on the improved roulette wheel and optimizes the regression parameters using the HPO-DOA algorithm, thereby improving the convergence speed and network accuracy of the BLS. Then, using the nonlinear mapping ability and adaptive ability of the improved BLS network, the results of the reliable GS-STSCKF algorithm are corrected from the compensation perspective to optimize and correct the linear approximation error caused by the pseudo-matrix construction, further improving the estimation accuracy of the filtering algorithm. The overall block diagram of the innovative work in this embodiment is as Figure 8 shown.

[0325] To verify the effectiveness and superiority of the algorithm in this embodiment, an unmanned ship motion model is used in the experiment. The initial state \(x_0\) and the initial covariance matrix \(P_0\) of the unmanned ship are respectively:

[0326] \(x_0 = [0\ 0\ 0.707\ 0.707\ 4.14\ 45\ 0.01\pi]\ (109)\)

[0327] \(P_0 = diag\{30,30,5,5,2,3,3\}\ (110)\)

[0328] where \(diag\) represents a diagonal matrix.

[0329] The process noise and the measurement noise are both represented by a Gaussian mixture model as follows:

[0330]

[0331]

[0332] In the formula, each weight parameter is set as: β k = 0.3, γ k = 0.4, and the remaining parameters are set as:

[0333]

[0334] According to Q 1 and its corresponding weight β k , generate β k × n process noise data. According to Q 2 and its corresponding weight (1 - β k ), generate (1 - β k ) × n process noise data. The combination of the two generates n process noise data, which is the set true noise data. Similarly, n true measurement noise data are generated. The expression for generating the noise data is shown in Equation (114).

[0335]

[0336] In the above formula, n = 50, sqrtm(·) represents taking the square root of a matrix, and rand(a, b) represents generating a random number with a rows and b columns.

[0337] To verify the ability of the multi - algorithm fusion method (BLS - trusted GS - STSCKF algorithm, BCGSSTSCKF) based on the coupling of GS - STSCKF and credibility theory proposed in this embodiment to estimate the noise covariance sub - term and the ship motion state, it is compared with the improved GS - CKF algorithm and the credibility - based and IGS - SCKF filtering algorithm (EGSSCKF) using the EM - PSO algorithm as the optimization algorithm, and the credibility - based and IGS - SCKF filtering algorithm (HGSSCKF) using the HPO - DOA algorithm as the optimization algorithm. For the selection of the fading factor matrix calculation method in the algorithm of this embodiment, considering that the solution of the direct matrix solution method is a special solution among many solutions and is not necessarily the optimal solution, the matrix trace solution method is selected in the experimental part.

[0338] For the two optimization algorithms of HPO - DOA and EM - PSO, the population size is set to 20, and the maximum number of iterations is set to 50. First, 50 Monte Carlo experiments are used to verify the advancement and effectiveness of their estimation of the noise covariance sub - term. The error curves of the noise covariance sub - terms estimated by the four algorithms and the true noise covariance sub - term are as Figures 9 to 12 shown. The mean square error results of the noise covariance sub - terms estimated by the four algorithms are shown in Tables 1 - 4. From Figures 9 to 12, as can be seen from Tables 1 to 4, the multi-algorithm fusion USV state estimation method based on the coupling of GS-STSCKF and credibility theory proposed in this embodiment can estimate each noise covariance sub-item more accurately than the other three algorithms. Therefore, it can be proved from the simulation experiment results that the algorithm proposed in this embodiment has better performance in estimating each noise covariance sub-item. The estimation error results of the unmanned ship motion states of the four algorithms are as Figure 13 shown, and the root mean square error results are shown in Table 5.

[0339] From Figure 13 , as can be seen from Table 5, the multi-algorithm fusion USV state estimation method based on the coupling of GS-STSCKF and credibility theory proposed in this embodiment has smaller state error results than the other three algorithms and can estimate the operation state of the unmanned ship more accurately. Therefore, when using the filtering algorithm designed in this embodiment, the state estimation performance is better.

[0340] Table 1 Estimation Error of Process Noise Covariance Component 1

[0341]

[0342] Table 2 Estimation Error of Process Noise Covariance Component 2

[0343]

[0344] Table 3 Estimation Error of Observation Noise Covariance Component 1

[0345]

[0346] Table 4 Estimation Error of Observation Noise Covariance Component 2

[0347]

[0348] To verify the effectiveness of the new algorithm in actual engineering applications, in this embodiment, the speed and heading data collected when the IBOAT electric-driven unmanned ship is traveling at a constant speed in the experimental water area and the latitude arc length, longitude arc length, heading change rate, and the east and north components of the sea current speed simulated by using the observation models in Equations (5) and (6) and their corresponding measurement noise covariances and other relevant parameters are used for algorithm verification and comparison. The algorithms compared in the actual data experiment are: STSCKF algorithm, robust STSCKF algorithm (RSTSCKF), adaptive robust STSCKF algorithm (IARSTSCKF), and strong tracking cubature particle filter algorithm (ISTCPF).

[0349] The specific experimental scenario is as Figure 14 shown. Given that the initial speed of the electric-driven unmanned ship is 5 m / s, the initial state x0 of the electric-driven unmanned ship is set as:

[0350] x0=[0 0 0.707 0.707 4.14 45 0.01π] T (115)

[0351] Both process noise and measurement noise are represented by Gaussian mixture models as follows:

[0352]

[0353] The process noise weight parameter is set as: β k =0.3. The true process noise covariance components are set as:

[0354]

[0355] The comparison results of different algorithms on actual data are as follows: Figure 15 As shown in Table 6. Figure 15 As shown in Table 6, in terms of the processing performance of actual data, the BLS-trusted GSSTSCKF filtering algorithm of this embodiment is superior to other types of STSCKF algorithms, further confirming the advancement and superiority of the algorithm of this embodiment. The processing time of different algorithms for actual data is shown in Table 7. Figure 14 The sampling period of the measured data is 50ms (0.05s). That is, the sensor outputs 20 sampled data per second. The algorithm of this embodiment takes 2.08 seconds to process 200 data, and can process about 96 data per second. Therefore, the estimated time of 2.08 seconds meets the real-time requirements in the actual dynamic environment.

[0356] Table 5 Root mean square error of estimation results of different algorithms

[0357]

[0358] Table 6 Root mean square error of actual state estimation effect of different algorithms

[0359]

[0360] Table 7 Processing time of different algorithms for actual data

[0361]

[0362] Regarding the issue of whether the substitution between sacrificing real-time performance and improving accuracy is worthwhile, taking the processing results of speed data as an example, as can be seen from Table 6 and Table 7, compared with the relatively advanced existing ISTCPF algorithm, the accuracy of the algorithm in this embodiment has increased by 59%, and the real-time performance has decreased by 6%. Compared with the traditional STSCKF algorithm, the accuracy of the algorithm in this embodiment has increased by 80%, and the real-time performance has decreased by 28%. From the perspective of the percentage comparison of accuracy and real-time performance, although the algorithm in this embodiment sacrifices some real-time performance, the improvement in estimation accuracy is more remarkable.

Claims

1. A multi-algorithm fusion USV state estimation method based on GS-STSCKF and credibility theory coupling for nonlinear non-Gaussian systems, characterized by: include: Input the sensor's measurement information into the trusted GS-STSCKF algorithm to obtain the filtering result; At the same time, the difference between the filtered state estimate and the state one-step prediction is input into the improved BLS network, and the error between the filtered result and the actual value is output; The filtering result output by the trusted GS-STSCKF algorithm is summed with the output of the improved BLS network to obtain the estimated state value of the USV; The trusted GS-STSCKF algorithm is obtained by following the steps below: The existing STSCKF filtering algorithm is improved to obtain an improved STSCKF filtering algorithm, the improvements include: directly applying the fading factor matrix to the adjustment of the covariance matrix of the process noise, that is, differentially adjusting each element in the covariance matrix of the process noise through the multiple fading factors in the fading factor matrix; The existing Gaussian fitting technology is improved to obtain an improved Gaussian fitting technology; the improved Gaussian fitting technology is used to enhance the non-Gaussian processing capability of the improved STSCKF filtering algorithm to obtain the GS-STSCKF algorithm; A trust factor is constructed on the GS-STSCKF algorithm to obtain a credible GS-STSCKF algorithm. The credible GS-STSCKF algorithm obtains the condition of a nonlinear non-Gaussian filter based on the trust factor. The condition is combined with the range of the trust factor value and the HPO-DOA algorithm to adjust the covariance of the observation noise in real time.

2. According to claim 1, a multi-algorithm fusion USV state estimation method for nonlinear non-Gaussian systems based on GS-STSCKF and credibility theory coupling is characterized by: The improved STSCKF filtering algorithm is improved to obtain an improved STSCKF filtering algorithm, and the specific operations include: The nonlinear non-Gaussian system is modeled and the following discrete nonlinear non-Gaussian system model is obtained: x k+1 =f k (x k )+w k+1,k (1) z k+1 =h k (x k )+v k (2) Among them, k represents the time, x k ∈R n represents the system state vector, f k :R n →R n 、h k :R n →R p Respectively represent the system state matrix and measurement matrix, w k+1,k ∈R n represents non-Gaussian process noise, z k ∈R p is the observation vector, v k ∈R p represents non-Gaussian observation noise, and the state noise and measurement noise are independent of each other; The state vector x of the unmanned ship motion model 无人船k , the state transfer function f(·) are: In the formula, and λ k are the latitude arc length and longitude arc length of the unmanned ship’s navigation, and are the east and north components of the water velocity, v k is the speed of the unmanned boat relative to the water flow, H k is the heading, Ω k is the heading change rate, β * is the anti-correlation time of the water flow, and the heading H k Related, β *-1 =15n mile / H, T represents the sampling period; The observation vector z of the unmanned ship motion model 无人船k And the observation matrix G are: G=I 7×7 (6) In the formula, and The latitude arc length, longitude arc length and speed of the unmanned ship measured by the global satellite navigation system. and are the heading and heading change rate of the electric-driven unmanned ship measured by the gyroscope, and They are respectively the east component and the north component of the water velocity measured by the current meter; The improvement of the existing STSCKF filtering algorithm includes: The calculation formula of the one-step prediction covariance in the existing STSCKF filtering algorithm is modified as follows: in, is the original fading factor matrix L k The vanishing factor matrix obtained by the Cholesky triangular decomposition technique is expressed as: in, Finally, the fading factor matrix is ​​calculated for: The fading factor matrix is ​​directly applied to the adjustment of the covariance matrix of the process noise, including: the fading factor matrix contains n different fading factors, and each element in the covariance matrix of the process noise is differentially adjusted through multiple fading factors.

3. The multi-algorithm fusion USV state estimation method for nonlinear non-Gaussian systems based on GS-STSCKF and credibility theory coupling according to claim 2 is characterized by: Follow these steps to calculate the vanishing factor matrix Specifically include: The pseudo observation matrix based on SCKF is expressed as: The pseudo-state transition matrix is ​​expressed as: F k|k-1 =N k (M k-1 ) -1 (12) in, M k-1 =chol(P k-1|k-1 )(15) At this time, the recursion in the filter is simplified to: Combining formula (17) and formula (19), the linearized one-step prediction covariance matrix of the STSCKF filtering algorithm can be obtained as follows: Another representation of the known orthogonality principle is: Among them, V k represents the innovation covariance matrix; Therefore, by substituting formula (18) into formula (22), we can obtain: Substituting formula (21) into formula (23), we can obtain: Based on formula (24), the direct matrix solution algorithm is used to solve the fading factor matrix 4. The multi-algorithm fusion USV state estimation method for nonlinear non-Gaussian systems based on GS-STSCKF and credibility theory coupling according to claim 3 is characterized by: The direct matrix solution algorithm is used to solve the vanishing factor matrix Specifically include: Thanks to Q k,k-1 It is a positive symmetric matrix. According to the Cholesky triangulation decomposition idea, it can be decomposed into: Substituting formula (25) into formula (24) yields: make: Then, formula (26) becomes: From formula (28), we can get the matrix A k ; Then, using pseudo-inverse technique, we can get H k The pseudo-inverse matrix for: Finally, the decomposed fading factor matrix is ​​obtained as follows: Assume that in general, when the dimensions of the observation vector and the state vector are different, that is, p≠n, and p<n, the matrix A k is a p×n matrix, no longer a square matrix, and S k is a p×p matrix; Solving for matrix A k The method is: First, S is decomposed by Cholesky decomposition. k Decomposed into a p×p matrix B k and its transpose, that is: Next, in the p-dimensional matrix B k Add a full 0 column at the end to make it n columns, and the new matrix is ​​matrix A. k , which satisfies the expression 5. The multi-algorithm fusion USV state estimation method for nonlinear non-Gaussian systems based on GS-STSCKF and credibility theory coupling according to claim 3 is characterized by: Based on formula (24), the direct matrix solution algorithm is used to solve the fading factor matrix It can be replaced by: Based on formula (24), the matrix trace solution algorithm is used to solve the fading factor matrix The matrix trace solution algorithm is used to solve the vanishing factor matrix Specifically include: Using pseudo-inverse technique, we can get H k and The pseudo-inverse matrix and in Calculated by formula (29), The calculation formula is as follows: So formula (24) becomes: By finding the traces on both sides of formula (33), we can obtain: make: Therefore, formula (34) can be simplified to: Substituting formula (10) into formula (36), we obtain: According to formula (37), we can get: in, Expressed as the process noise covariance matrix Q k,k-1 The i-th row and i-th column element of Represented as C k The element in the i-th row and i-th column of ; Thus, the vanishing factor matrix L before and after decomposition can be obtained k and 6. The multi-algorithm fusion USV state estimation method for nonlinear non-Gaussian systems based on GS-STSCKF and credibility theory coupling according to claim 3 is characterized by: The GS-STSCKF algorithm specifically includes: Step 1: Initialization: Set the initial time point k = 0 and define the initial state p(x0|z -1 )=p(x0), and use it as N k|k-1 The sum of Gaussian subterms; Using the EM algorithm, the initial probability density function p(x0), the process noise probability density function p(w k ) and the measurement noise probability density function p(υ k ) are expressed in the form of Gaussian sum as follows: in, and is the weight of its corresponding Gaussian sub-item, and there is Step 2: Distributed filtering: Use the Gaussian sum form of formula (42) to approximate the filtering probability density function: Among them, measurement And: N k|k =N k|k-1 ·r k ; In addition, based on the predicted state mean Prediction covariance matrix and the mean value of the measurement noise and the measurement variance matrix Use the STSCKF algorithm to calculate the i-th Gaussian subterm The filtered mean and the covariance matrix At this time, the measurement likelihood function of the state estimation of the i-th Gaussian sub-item can be expressed as: The calculation method of indicators j and l is: Where i = 1, 2, ..., N k|k ; j = 1, 2, ..., N k|k-1 ; l=j=1,2,…,r k ;symbol Indicates floor operation; Filter weights corresponding to Gaussian sub-items It is necessary to integrate three pieces of information, namely the prediction filter weights The weight of the probability density function of the measurement noise And the likelihood function of the state estimate Therefore The calculation method is: Step 3: Global point estimation: After each Gaussian sub-item obtains the filtering state at time k, the global filtering mean at time k is calculated using formula (46) and formula (47): and the covariance matrix P k|k : Similarly, the state estimates at other time points can also be calculated using formulas (46) to (47); Step 4: Simplification control: After calculating the estimated mean and covariance of each Gaussian sub-item at time k, first retain only all Gaussian components with magnitude greater than G′: Among them, G′ is the weight of the Gth Gaussian component after the weights are sorted from large to small; Among the retained Gaussian components, find the component with the largest weight: And find all Gaussian components that meet the conditions according to the following criteria: in, These Gaussian components are combined into a Gaussian distribution by weighted averaging, and its mean and covariance are: Delete all Gaussian components that have been fused, and then repeat the above steps until all Gaussian components have completed the operation; finally, only the first S sub-Gaussian distributions with relatively large weights are retained, and the corresponding weights are normalized again; Step 5: Prediction update: The predicted state probability density function is expressed as follows: in, It can be seen that the prediction weight corresponding to the Gaussian sub-item The calculation of combines two pieces of information, namely the filter weights and the weight of the process noise probability density function In addition, based on the estimated state mean Estimated variance matrix Process noise mean Process noise covariance matrix Use the STSCKF algorithm to calculate the i-th Gaussian subterm The predicted mean and the covariance matrix In addition, indicators c and d are calculated as follows: Where i = 1, 2, ..., N k+1|k ; c=1,2,…,N k|k ; d=1,2,…,q k ; At this point, the state filtering process at time k has been completed, and the time point update k=k+1 is set, and the process jumps to Step 2 to implement step-by-step state filtering.

7. The multi-algorithm fusion USV state estimation method for nonlinear non-Gaussian systems based on GS-STSCKF and credibility theory coupling according to claim 6 is characterized by: The trust factor constructed on the GS-STSCKF algorithm is expressed as: in, yes The 2-norm of The value range of is (0, +∞), so formula (65) normalizes it; t(k) is a trust factor with a value range of 0 to 1, which is used to measure the closeness of FMSE and TMSE of each sub-filter; The conditions for obtaining a nonlinear non-Gaussian filter based on the trust factor include: Innovation of known sub-filters for: The covariance matrix of is: The superscript f indicates a calculated value; however Not really The true value of the covariance matrix of ; The true covariance is: The superscript m indicates the true value; From formula (67) and formula (68), we get: For the Kalman sub-filter containing inaccurate observation noise covariance in the Gaussian sub-term: is always a positive definite matrix, then is a positive definite matrix; if is always a negative definite matrix, then is a negative definite matrix; When there is a mismatched observation noise covariance, using formula (58) and formula (62), we can get: According to formula (69) and formula (62), we can get: in, and can be calculated or estimated; The Sage-Husa adaptive Kalman filtering method is used to estimate the observation noise covariance. The Sage-Husa adaptive Kalman filtering method is briefly described as: in, Obtained by the time-varying noise statistical estimator: In the formula, For the forgetting factor; The indicators h and o are calculated as follows: According to formula (72), we can get: Therefore, according to formula (65), the estimated value of t(k0) is From formula (77), we can get: According to formula (77), when hour, in, represents the observed noise covariance satisfied by the Gaussian subterm; From formula (83), we can see that the Sage-Husa adaptive Kalman filtering algorithm is hour, Converge to Right now is the convergence estimate of the algorithm; therefore, by adjusting To satisfy: Among them, st represents the constraint condition, and the calculation of index l is shown in formula (44); In each Gaussian subterm, find the observation noise covariance subterm that satisfies equation (84); The nonlinear non-Gaussian system is composed of N k|k Therefore, according to formula (84), the condition for designing a high-performance filter for a nonlinear non-Gaussian system is: The method of using the condition in combination with the range of the trust factor value and the HPO-DOA algorithm to adjust the covariance of the observation noise in real time specifically includes: The hunter-prey optimization algorithm and the wild dog optimization algorithm are combined to achieve simultaneous estimation of the covariance sub-terms of multiple observation noises, thereby obtaining the required Specific operations include: The process of HPO-DOA algorithm is as follows: 1) In the defined interval [x min ,x max ] Randomly generate population individuals, and put the individuals estimated by the EM algorithm into the randomly generated initial population; 2) Constructing the fitness function Substitute the population individuals as the observed noise covariance corresponding to each Gaussian sub-item into the filtering algorithm, and obtain the corresponding Gaussian sub-item through equations (67) and (68): and And calculate the fitness value; 3) Find the global optimal position according to each fitness value; 4) distinguishing between hunters and prey among individuals in a population; 5) Update individuals in the population; 6) Calculate the fitness value of the updated individual and find the global optimal position; 7) Determine whether the number of iterations reaches the upper limit. If so, output the corresponding optimal individual as the observed noise covariance estimate of each Gaussian sub-item Otherwise go to step 4); 8) Calculate t(k) according to formula (65); 9) Determine whether t(k) is greater than 0.

9. If so, output Applied to the GS-STSCKF algorithm; otherwise Put it into the initial population for further optimization.

8. The multi-algorithm fusion USV state estimation method for nonlinear non-Gaussian systems based on GS-STSCKF and credibility theory coupling according to claim 7 is characterized by: The improved BLS network is obtained by following the steps below: The genetic algorithm optimizes the BP neural network algorithm to optimize the initial weights in the BLS network, and the determined fitness function is: Among them, T i represents the actual output value of the i-th training sample, Y i It represents the expected output value of the i-th training sample; The roulette wheel selection method using the following steps is used in the genetic algorithm to optimize the BP neural network algorithm: 1) Arrange the individuals according to their fitness values, add up all the fitness values ​​obtained, and record the sum as S; 2) Generate a random number M, M∈(0,S); 3) Starting from the fitness function value of the first individual, sum it with the fitness function values ​​of the following individuals in turn. If the accumulated value exceeds M, stop. The last individual whose fitness function value is accumulated is the selected parent. 4) These selected individuals are taken out separately, and steps 2) and 3) are repeated until a sufficient number of parents are selected; Let Y p is the predicted output, Y is the actual value, N is the number of samples, and Y p Derived from the actual input and weight: Y p =M(λI+M T M) -1 M T M (106) In the formula, λ is a regression parameter with adjustable value; Define the objective function with respect to the regression parameter λ: The HPO-DOA optimization algorithm is used to solve the objective function of the regression parameter λ, including: 1) In the defined interval [x min ,x max ] Randomly generate population individuals; 2) Constructing fitness function And calculate the fitness value; 3) Find the global optimal position according to each fitness value; 4) distinguishing between hunters and prey among individuals in a population; 5) Update individuals in the population; 6) Calculate the fitness value of the updated individual and find the global optimal position; 7) Determine whether the number of iterations reaches the upper limit. If so, output the corresponding optimal λ value; otherwise, go to step 4).

Images