A UUV target tracking method based on generalized maximum correlation entropy Kalman filtering

Abstract

This invention discloses a UUV target tracking method based on generalized maximum correlation entropy Kalman filtering, comprising the following steps: S1. Establishing the motion equations of the UUV, where UUV refers to an unmanned underwater vehicle; S2. Establishing the observation equations of the UUV; S3. Processing the data using a Kalman filtering algorithm based on weighted least squares and generalized maximum correlation entropy to achieve UUV tracking and localization. This invention applies the Kalman filtering algorithm based on weighted least squares and generalized maximum correlation entropy to UUV target tracking, using weighted least squares to handle process noise and generalized maximum correlation entropy to handle observation noise. It exhibits strong robustness to different types of non-Gaussian noise and can achieve excellent tracking results in complex marine environments.

Description

Technical Field

[0001] This invention applies to the field of unmanned underwater vehicles, and in particular relates to a UUV target tracking method based on generalized maximum correlation entropy Kalman filtering. Background Technology

[0002] Unmanned Underwater Vehicles (UUVs) have become a research hotspot in the marine field this year, playing a crucial role in underwater exploration, underwater video recording, and search and reconnaissance missions. To meet the requirements for normal operation in the deep sea, obtaining accurate and convenient location information is a primary prerequisite. Therefore, whether for long-range escort of UUVs or supporting them in completing specific underwater tasks, tracking and locating moving UUVs has always been an extremely important issue in the marine field. Currently, UUV tracking and locating mainly relies on detecting the acoustic radiation of UUVs. Ultra-short baseline (USBL) positioning systems utilize distance and heading angle measurements for underwater target tracking and locating, and are widely used in UUV positioning and tracking due to their high integration and ease of operation.

[0003] However, due to the dynamic nature of the complex marine environment (temperature, salinity, and density, etc.), the USBL positioning system for tracking and locating UUVs can be severely affected by various errors. Kalman filtering (KF) is typically used to improve the accuracy and reliability of underwater tracking and positioning. Traditional Kalman filtering uses the minimum mean square error as the criterion under the assumptions of a linear system and Gaussianness. However, the marine environment does not satisfy the Gaussian noise assumption, leading to performance degradation of traditional Kalman filtering in the presence of non-Gaussian noise, especially impulse noise. The main reason is that the minimum mean square error criterion can only use second-order statistics of the data, making it highly sensitive to outliers in noise, thus degrading the robustness of traditional Kalman filtering in most practical scenarios. Summary of the Invention

[0004] The purpose of this invention is to overcome the shortcomings of the prior art and provide a UUV target tracking method based on generalized maximum correlation entropy Kalman filtering. It uses weighted least squares to handle process noise and generalized maximum correlation entropy to handle observation noise. It has strong robustness to different types of non-Gaussian noise and can achieve excellent tracking results in complex marine environments.

[0005] The objective of this invention is achieved through the following technical solution: a UUV target tracking method based on generalized maximum correlation entropy Kalman filtering, comprising the following steps:

[0006] S1. Establish the motion equations of the UUV:

[0007]

[0008] In the formula, the left side of the equal sign It is the state of the target at time k, x k ,y k ,z k Let k be the position of the target in the coordinate system at time k. Let be the velocity of the target at time k, and T be the observation time interval for acquiring the target's state. During operation, the UUV will inevitably be affected by external environmental factors. These influencing factors are considered as process noise and modeled accordingly, so process noise W is included. k UUV stands for Unmanned Underwater Vehicle;

[0009] S2. Establish the observation equation for UUV:

[0010]

[0011] During the observation process, the USBL positioning system, or Ultra-Short Baseline Positioning System, was used. The position of the target at time k obtained by the sensor was denoted as... The observation process of a USBL positioning system is affected by noise error, which is defined as V. k ;

[0012] S3. A Kalman filter algorithm based on weighted least squares and generalized maximum correlation entropy is used to process the data to achieve UUV tracking and localization:

[0013] S301: Select the shape parameter α and the scale parameter β, and set the initial state estimate. and the initial covariance matrix P 0|0 ;

[0014] S302: Obtain the predicted values ​​and the error covariance matrix between the actual and predicted values:

[0015]

[0016] P k|k-1 =A k P k-1|k-1 A k T +Q k-1

[0017] Among them, A k It is the system's state transition matrix. It is the best estimate of the state at time k-1. P is the predicted value of the state at time k.k|k-1 It is the error covariance matrix between the true value and the predicted value at time k, i.e., the prior estimated covariance matrix, P. k-1|k-1 Q is the error covariance matrix between the true value and the estimated value at time k-1. k-1 It is the covariance matrix of the process noise at time k-1;

[0018] S303: By establishing a cost function, the state at time k is optimally estimated to obtain the estimated state.

[0019]

[0020] in,

[0021]

[0022] in, γ is the estimated state at time k. Since the state includes the position and velocity of the target at time k, target tracking of the UUV can be achieved based on the estimated state. γ = α / [2βΓ(1 / α)] is the normalization constant, exp is an exponential function with base e, and λ = 1 / β α It's a kernel parameter, C k R is the observation matrix of the system at time k. k It is the covariance matrix of the observation noise at time k;

[0023] S304: Define the filter gain matrix By using the matrix inversion lemma, and assuming the estimated state is obtained, the filter gain matrix K can be solved. k ;

[0024] S305: Update the posterior estimate of the covariance matrix.

[0025]

[0026] Among them, P k|k It is the posterior estimated covariance matrix at time k, used for iterative updates at time k+1.

[0027] The beneficial effects of this invention are as follows: This invention introduces the cost function based on weighted least squares and generalized maximum correlation entropy into Kalman filtering and applies it to the target tracking and positioning algorithm of UUVs. It establishes a UUV motion model and an observation model. By filtering the UUV position information observed by the USBL system, the influence of external process noise and observation noise is avoided, and robust tracking and positioning is achieved. Attached Figure Description

[0028] Figure 1 This is a schematic diagram of the method flow of the present invention;

[0029] Figure 2This is a flowchart illustrating the Kalman filtering method based on weighted least squares and generalized maximum correlation entropy in an embodiment of the present invention.

[0030] Figure 3 This is a schematic diagram of the tracking error when sampling 300 samples in an example of an embodiment of the present invention;

[0031] Figure 4 This is a schematic diagram of the tracking error when selecting different shape parameters α in an example of an embodiment of the present invention, with 300 samples.

[0032] Figure 5 This is a schematic diagram of the tracking error when selecting different shape parameters β in an example of an embodiment of the present invention, with 300 samples. Detailed Implementation

[0033] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings, but the scope of protection of the present invention is not limited to the following description.

[0034] like Figure 1 As shown, a UUV target tracking method based on generalized maximum correlation entropy Kalman filtering includes the following steps:

[0035] S1. To simulate the motion of a UUV underwater, it is necessary to establish a motion equation, that is, to model the motion law of the target. Assuming that the target moves at a constant velocity in three-dimensional space, the motion equation of the target at time k+1 in discrete time can be expressed as:

[0036]

[0037]

[0038]

[0039]

[0040]

[0041]

[0042] In the formula, x k ,y k ,z k Let k be the position of the target at time k. Let be the velocity of the target at time k, and T be the observation time interval for acquiring the target's state. In actual operation, UUVs are inevitably affected by external environmental factors. These influencing factors can be modeled as process noise, so process noise W is added. k .

[0043] The above equations of motion can be expressed in matrix form as follows:

[0044]

[0045] In the formula, the left side of the equal sign The state of the target at time k

[0046] S2. Construct the observation equations, which are model assumptions for obtaining the UUV locations of the USBL system:

[0047]

[0048] In practice, the observation process of the USBL system is affected by noise error. This invention defines this observation noise as V. k .

[0049] S3. A cost function based on weighted least squares and generalized maximum correlation entropy is introduced into Kalman filtering and applied to the target tracking and localization algorithm of UUVs. This Kalman filtering method is described below:

[0050] For the Kalman filter algorithm, the state-space expression of the linear system is denoted as:

[0051] x k =A k x k-1 +w k ,

[0052] y k =C k x k +v k ,

[0053] Where the subscript k represents the k-th time point, It is the state value at time k. These are the observations at time k. It is the state transition matrix. It is the observation matrix. It's process noise. This is observation noise. k and v k It is zero-mean noise and they are uncorrelated. The covariance matrix of the two can be defined as follows:

[0054]

[0055]

[0056] Due to the presence of non-Gaussian noise in real-world scenarios, the performance of Kalman filtering methods based on the minimum mean square error criterion degrades. Furthermore, the diverse distribution of noise leads to significant performance fluctuations in Kalman filtering methods based on maximum correlation entropy. Therefore, this invention proposes a Kalman filtering method based on weighted least squares and generalized maximum correlation entropy to flexibly handle different types of non-Gaussian noise in real-world applications.

[0057] This invention proposes a novel cost function that uses weighted least squares to handle process noise and weighted least squares and generalized maximum correlation entropy to handle observation noise. The expression is as follows:

[0058]

[0059] Where m and n are weight values. It is an estimate of the state at time k. P is the predicted value of the state at time k. k|k-1 G is the covariance matrix between the actual and predicted values ​​at time k, i.e., the prior estimated covariance matrix. α,β () is the Gaussian density function, α is the shape parameter, and β is the scale parameter.

[0060] The optimal estimate of the current state is as follows:

[0061]

[0062] Here, sign() is the sign function, and the current state optimal estimation decision is made when the cost function J... L When the minimum value is reached, the estimated state at this point For optimal estimation, the specific method is the cost function J. L right Taking the derivative, when the derivative equals 0, the cost function is considered to have reached its minimum value. The above formula guarantees that the estimated value obtained by this method is the optimal estimate of the true value. Let n = 1, m = αλ / 2, simplifying and rearranging, we can obtain...

[0063]

[0064] in,

[0065]

[0066] Let the filter gain matrix equals K k By using the matrix inversion lemma, we can obtain the following:

[0067] like Figure 2 As shown, the Kalman filtering method proposed in this invention mainly involves the following steps:

[0068] S301. Select appropriate shape parameter α and scale parameter β, and set the initial state estimate. and the initial covariance matrix P 0|0 .

[0069] S302. Use the following formula to obtain the predicted values ​​and the prediction error covariance matrix.

[0070]

[0071] P k|k-1 =A k P k-1|k-1 A k T +Q k-1

[0072] Among them, A k It is the system's state transition matrix. It is the best estimate of the state at time k-1. P is the predicted value of the state at time k. k|k-1 It is the error covariance matrix between the true value and the predicted value at time k, i.e., the prior estimated covariance matrix, P. k-1|k-1 Q is the error covariance matrix between the true value and the estimated value at time k-1. k-1 It is the covariance matrix of the process noise at time k-1.

[0073] S303. By establishing a cost function and performing an optimal estimation of the current state, the estimated state can be obtained.

[0074]

[0075] in,

[0076]

[0077] in, γ is the estimated state at time k, which is the UUV target position obtained by this method at this time, thus realizing UUV target tracking. γ=α / [2βΓ(1 / α)] is the normalization constant, exp is an exponential function with base e, and λ=1 / β α It's a kernel parameter, C k R is the observation matrix of the system at time k. k It is the covariance matrix of the observation noise at time k;

[0078] S304: Based on the estimated state above, the Kalman filter gain matrix can be defined.

[0079]

[0080] S305. Use the following formula to update the posterior estimated covariance matrix.

[0081]

[0082] Among them, P k|k It is the posterior estimated covariance matrix at time k, which facilitates iterative calculation at time k+1.

[0083] The performance of the UUV target tracking method based on weighted least squares and generalized maximum correlation entropy Kalman filtering in the embodiments of the present invention is evaluated below with specific simulation examples.

[0084] In the embodiments of this application, the constant velocity model is a common target motion model, which assumes that the target moves in a straight line at a certain speed. The weighted least squares and generalized maximum correlation entropy Kalman filtering (WGMCKF) methods of this invention are compared with the traditional Kalman filtering (KF) and maximum correlation entropy Kalman filtering (MCKF) algorithms, respectively.

[0085] The state-space expression for the constant velocity model is shown below.

[0086]

[0087]

[0088] Where T = 1 represents the measurement time interval, w k and v k It consists of process noise and observation noise that conform to Gaussian mixture noise, expressed as follows:

[0089] w i:k ~0.9N(0,0.01)+0.1N(0,1),i=1,2,...,6

[0090] v j:k ~0.9N(0,0.01)+0.1N(0,10),j=1,2,3

[0091] Figure 3 This is a schematic diagram of the tracking error when the shape parameter α = 1.0 and the scale parameter β = 2.0 are selected in an example of this invention, with 300 samples. Qualitative comparative analysis of the tracking error trend shows that, compared to traditional Kalman filtering and maximum correlation entropy Kalman filtering algorithms, weighted least squares and generalized maximum correlation entropy Kalman filtering methods can better handle the interference of non-Gaussian noise encountered during UUV target tracking.

[0092] Figure 4This is a schematic diagram illustrating the tracking error in an embodiment of the present invention, where shape parameters α = 1.0, 1.5, 2.0, 2.5, 4.0 and scale parameter β = 2.0 are selected, and 300 samples are collected. Analysis from the perspective of steady-state tracking error shows that, for observation noise, the smaller the value of the shape parameter, the better the target tracking algorithm performs.

[0093] Figure 5 This is a schematic diagram illustrating the tracking error when the scale parameter β = 0.7, 1.0, 2.0, 3.5, and 5.0, the shape parameter α = 2.0, and 300 samples are collected, according to an example of this invention. Analysis from the perspective of steady-state tracking error shows that for observation noise, the smaller the value of the scale parameter, the better the UUV target tracking algorithm performs. In summary... Figure 4 and Figure 5 As can be seen, both shape and scale parameters affect the algorithm performance in this embodiment. When dealing with different external noises, selecting appropriate parameters can enable the algorithm to achieve the best tracking performance.

[0094] The foregoing description illustrates and describes a preferred embodiment of the present invention. However, as previously stated, it should be understood that the present invention is not limited to the forms disclosed herein and should not be construed as excluding other embodiments. It can be used in various other combinations, modifications, and environments, and can be altered within the scope of the inventive concept described herein through the foregoing teachings or techniques or knowledge in related fields. Any modifications and variations made by those skilled in the art that do not depart from the spirit and scope of the present invention should be within the protection scope of the appended claims.

Claims

1. A UUV target tracking method based on generalized maximum correlation entropy Kalman filtering, characterized in that: Includes the following steps: S1. Establish the motion equations of the UUV, where UUV stands for Unmanned Underwater Vehicle; The equation of motion described in step S1 is: In the formula, the left side of the equal sign It is the state of the target at time k, x k ,y k ,z k Let k be the position of the target in the coordinate system at time k. Let be the velocity of the target at time k, and T be the observation time interval for acquiring the target's state. During operation, the UUV will inevitably be affected by external environmental factors. These influencing factors are considered as process noise and modeled accordingly, so process noise W is included. k ; S2. Establish the observation equations for UUVs; The observation equation mentioned in step S2 is: During the observation process, the USBL positioning system, or Ultra-Short Baseline Positioning System, was used. The position of the target at time k obtained by the sensor was denoted as... The observation process of a USBL positioning system is affected by noise error, which is defined as V. k ; S3. A Kalman filter algorithm based on weighted least squares and generalized maximum correlation entropy is used to process the data to achieve UUV tracking and positioning; Step S3 includes the following sub-steps: S301: Select the shape parameter α and the scale parameter β, and set the initial state estimate. and the initial covariance matrix P 0|0 ; S302: Obtain the predicted values ​​and the error covariance matrix between the actual and predicted values: P k|k-1 =A k P k-1|k-1 A k T +Q k-1 Among them, A k It is the system's state transition matrix. It is the best estimate of the state at time k-1. P is the predicted value of the state at time k. k|k-1 It is the error covariance matrix between the true value and the predicted value at time k, i.e., the prior estimated covariance matrix, P. k-1|k-1 Q is the error covariance matrix between the true value and the estimated value at time k-1. k-1 It is the covariance matrix of the process noise at time k-1; S303: By establishing a cost function, the state at time k is optimally estimated to obtain the estimated state. in, in, γ is the estimated state at time k. Since the state includes the position and velocity of the target at time k, target tracking of the UUV can be achieved based on the estimated state. γ = α / [2βΓ(1 / α)] is the normalization constant, exp is an exponential function with base e, and λ = 1 / β α It's a kernel parameter, C k R is the observation matrix of the system at time k. k It is the covariance matrix of the observation noise at time k; S304: Define the filter gain matrix By using the matrix inversion lemma, and assuming the estimated state is obtained, the filter gain matrix K can be solved. k ; S305: Update the posterior estimate of the covariance matrix. Among them, P k|k It is the posterior estimated covariance matrix at time k, used for iterative updates at time k+1.

2. The UUV target tracking method based on generalized maximum correlation entropy Kalman filtering according to claim 1, characterized in that: The cost function established in step S303 uses weighted least squares to process noise and generalized maximum correlation entropy to process observation noise, and is expressed as follows: Where m and n are weight values. It is an estimate of the state at time k. P is the predicted value of the state at time k. k|k-1 G is the covariance matrix between the actual and predicted values ​​at time k, i.e., the prior estimated covariance matrix. α,β () is the Gaussian density function, α is the shape parameter, and β is the scale parameter.

3. The UUV target tracking method based on generalized maximum correlation entropy Kalman filtering according to claim 1, characterized in that: In step S303, when performing optimal estimation of the state at time k, the state optimal estimation determination is based on the cost function J. L When the minimum value is reached, the estimated state at this point For optimal estimation, the specific method is the cost function J. L right Take the derivative; when the derivative equals 0, the cost function is considered to have reached its minimum value, expressed as: Where sign() is the sign function, let n = 1, m = αλ / 2, simplification and rearrangement yields... in,

Images