Self-adaptive pure azimuth pseudo-linear Kalman filtering tracking method

Abstract

The invention discloses a self-adaptive pure azimuth pseudo-linear Kalman filtering tracking method, and belongs to the technical field of pure azimuth target tracking, and the method comprises the steps: obtaining the observation information of target sonar detection on a target through ad hoc network communication; the residual error is updated in real time by using observation information, and the observation variance of Kalman filtering is corrected so as to position a target; and after the target is successfully positioned, the position of the target is corrected in real time by using adaptive pseudo-linear Kalman filtering. A traditional adaptive EKF algorithm is utilized to optimize and improve a traditional deviation compensation PLKF algorithm so as to perform stable and reliable target tracking.

Description

Technical Field

[0001] The invention belongs to the technical field of bearings-only target tracking, and more specifically, relates to an adaptive bearings-only pseudo-linear Kalman filter tracking method. Background Art

[0002] Passive sonar is one of the important sensors currently used for underwater target tracking. Passive sonar does not require active sound, is not easily discovered, and can detect long-distance targets. Passive sonar can detect targets from several kilometers to tens of kilometers away. Since the sound and characteristics of the target are not controlled by the sonar designer, the main observation information of passive sonar is the direction of the target relative to the sonar. The system only uses the target direction information to estimate the target's motion trajectory information in real time, including the target's position and speed. This process is called bearing-only tracking (BOT).

[0003] When the observation system is a linear system, it is assumed that the system noise strictly follows the normal distribution, and the Kalman filter is the optimal estimate of the system. When the observation system is a nonlinear system, the traditional Kalman filter is no longer applicable. The pure bearings tracking system is a typical nonlinear system. The main challenges of pure bearings tracking are nonlinear observation bias and poor observability of radial distance. Pure bearings tracking algorithm is an important research topic. The academic community has applied a variety of algorithms and their improved methods in this field. Currently, the commonly used algorithms include Extended Kalman Filter (EKF), Pseudolinear Kalman Filter (PLKF), Unscented Kalman Filter (UKF) and other algorithms. The EKF algorithm linearizes the observation equation by truncated Taylor series expansion, with small calculation amount, but when there is only nonlinear bearing information, it is easy to diverge and the accuracy is unstable; UKF needs to adjust multiple hyperparameters, the algorithm complexity is high, and the adaptability to the maneuvering environment is poor. PLKF is a linear recursive Bayesian estimation method. The algorithm uses a pseudolinear estimator (PLE) to replace nonlinear equations with pseudolinear equations. Compared with other algorithms used for pure bearings tracking, PLKF has low complexity and better tracking performance.

[0004] The commonly used adaptive method is the Sage-Husa-based novelty adaptive algorithm, which uses the novelty variance to infer the variance of the observation error, so as to match the error variance. When the actual noise is small, the algorithm may cause the matrix to be negatively definite during iteration, causing the filter to diverge. Since the pseudo-linear equation observation error variance of the traditional PLKF algorithm changes with the relative position of the platform and the target, the commonly used Sage-Husa adaptive filtering method cannot be directly applied to the PLKF algorithm.

[0005] In summary, among the traditional bearings-only target positioning methods, when the variance of the observation noise is known, the performance of the bias-compensated PLKF algorithm is better than that of the traditional EKF algorithm. However, the bias-compensated PLKF algorithm has poor adaptive performance to the observation noise, while the traditional adaptive EKF algorithm has better adaptive performance to the observation noise. Summary of the invention

[0006] In view of the above defects or improvement needs of the prior art, the present invention proposes an adaptive bearing-only pseudo-linear Kalman filter tracking method, which optimizes and improves the traditional deviation-compensated PLKF algorithm by using the traditional adaptive EKF algorithm.

[0007] To achieve the above object, the present invention provides an adaptive bearing-only pseudo-linear Kalman filter tracking method, comprising:

[0008] Obtain the target's observation information from sonar detection through ad hoc network communication;

[0009] The residual is updated in real time using the observation information to correct the observation variance of the Kalman filter to locate the target;

[0010] After successfully locating the target, the adaptive pseudo-linear Kalman filter is used to correct the target position in real time.

[0011] In some optional implementation schemes, the use of observation information to update the residual in real time and to correct the observation variance of the Kalman filter includes:

[0012] The observation information is filtered through PLKF, and the statistical deviation is calculated, and the deviation is compensated by PLKF;

[0013] The residual variance is calculated and variance adaptation is performed to correct the observation variance of the Kalman filter.

[0014] In some optional implementation schemes, the statistical residual variance is calculated and variance adaptation is performed to correct the observed variance of the Kalman filter, including:

[0015] Depend on To predict the state, Perform state covariance matrix prediction;

[0016] Depend on Equivalent observation variance estimation is performed by Get the Kalman gain matrix;

[0017] Depend on Status update by Update the state covariance matrix;

[0018] Depend on To compensate for the deviation, Estimation of observation position variance;

[0019] Among them, x k-1 is the target state vector at time k-1, F is the state transfer matrix, is the predicted state vector, P k-1 is the system covariance matrix at time k-1, W k-1 is the observation error covariance at time k-1, is the one-step prediction system covariance matrix, d k is the vector from the platform position to the target position, is the estimated variance of the observation noise, is the estimated value of the observation noise variance, K k is the compensation gain, H k is the estimated observation matrix of PLKF, is the result of RA-PLKF algorithm, is the deviation compensation amount, P k / k is the system covariance matrix at time k, M is the intermediate variable driving matrix, is the predicted position of the platform at time k, r k is the residual at time k, is the deviation compensation of the original observation value of PLKF.

[0020] In some optional implementations, after calculating the statistical residual variance and performing variance adaptation to correct the observed variance of the Kalman filter, the method further includes:

[0021] When over-compensation of filtering occurs, the SAM criterion is used for state estimation, and the PLKF filtering result is used as the target filtering state value.

[0022] In some optional implementations, when over-filtering compensation occurs, using the SAM criterion to perform state estimation and using the PLKF filtering result as the target filtering state value includes:

[0023] At time k, if Perform state prediction, where is the azimuth angle estimated by the adaptive compensation PLKF algorithm, is the actual observation azimuth angle between the observation platform and the target relative to the positive half axis of the X-axis, is the deviation threshold of RA-PLKF, The target estimated position for the SAM-RAPLKF algorithm, Estimated position for the target of RAPLKF.

[0024] In general, the above technical solutions conceived by the present invention can achieve the following beneficial effects compared with the prior art:

[0025] In the passive sonar pure bearing tracking scenario, in order to solve the problem of filter divergence in the traditional pseudo-linear Kalman filter algorithm when the observation noise is mismatched, the traditional deviation compensation method is optimized, and the RA-PLKF algorithm is proposed. The SAM criterion is used to further optimize its stability, and the SAM-RA-PLKF algorithm is proposed, which further improves the tracking accuracy and stability of the algorithm. Simulations show that in an environment where the observation noise variance is mismatched, the algorithm proposed in the present invention outperforms the commonly used pure bearing tracking method in performance and has reliable stability. The proposed adaptive pseudo-linear Kalman filter algorithm can provide reference significance for mobile platform target tracking in marine environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] Figure 1 It is a schematic diagram of a pure bearing target tracking scenario of a two-dimensional UUV provided by an embodiment of the present invention;

[0027] Figure 2 It is a schematic diagram of a SAM-RA-PLKF process provided by an embodiment of the present invention;

[0028] Figure 3 It is a schematic diagram of a basic simulation experiment environment provided by an embodiment of the present invention;

[0029] Figure 4 It is a platform trajectory and 10 target trajectory diagrams provided by an embodiment of the present invention;

[0030] Figure 5 This is a Kalman filtering principle provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0031] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0032] An adaptive bearing-only pseudo-linear Kalman filter tracking method of the present invention specifically includes:

[0033] Establishment of ad hoc network communication: The onshore control center and the unmanned platform establish a flexible communication network through wireless ad hoc network radio equipment. This ad hoc network structure can maintain stable communication in complex marine environments.

[0034] Initialization: It involves initial settings or configurations, inputting the initial state of the platform, and setting the initial detection state of the sonar. The unmanned platform starts the towed array sonar detection function and begins searching for underwater targets. At the same time, the shore control center starts the information processing software to prepare to receive and process data from the unmanned platform. The unmanned platform lowers the towed array cable and detects underwater targets through the sonar system.

[0035] Residual correction: Use the observed values ​​to update the residuals in real time and correct the observation variance of the Kalman filter.

[0036] Application of SAM-RA-PLKF Kalman filter: After successfully locating the target, the system uses the proposed improved adaptive pseudo-linear Kalman filter technology to make real-time corrections to the target position to improve the tracking accuracy.

[0037] In a two-dimensional environment, consider a single mobile platform tracking an underwater target in pure bearings. The UUV follows the Figure 1 The platform trajectory shown in the figure patrols the area, detects the position of the target entering the detection area, and tracks the target, such as Figure 1 shown.

[0038] The system state equation is:

[0039] x k =Fx k-1 +Gw k-1 (1)

[0040] In the formula, x k =[p k ,v k ] T is the target state vector at time k; p k =[p x,k ,p y,k ] is the position vector of the target at time k; v k =[v x,k ,v y,k ] is the velocity vector of the target at time k; F is the state transfer matrix, G is the process noise driving matrix, w k is the process noise, assuming that each item is independent and obeys the normal distribution, w k ~Ν(0,Qk ), It represents the random deviation of the target state. T is the sampling time interval.

[0041] The system observation equation is:

[0042]

[0043] In the formula, like Figure 1 ,θ k is the actual relative azimuth angle, is the actual observation azimuth angle between the observation platform and the target relative to the positive half axis of the X-axis; r k =[r x,k ,r y,k ] is the position vector of the platform at time k; v k is the azimuth observation noise, Represents the random deviation of the observed orientation.

[0044] The process of the PLKF algorithm is as follows.

[0045] First, the observation equation is transformed into the following form:

[0046]

[0047] After expansion and approximation, we get:

[0048]

[0049] In the formula,

[0050] Here we record the equivalent observation value Equivalent observation matrix Then we have:

[0051] z k =H k x k +η k (5)

[0052] In the formula, the equivalent noise is:

[0053] η k =-||d k ||sin v k (6)

[0054] Where, d k =p k -r k , which represents the vector from the platform position to the target position. The equivalent noise variance is:

[0055]

[0056] Because v k is zero-mean Gaussian noise:

[0057]

[0058] When the azimuth noise is small, That is, at this time there is an equivalent observation noise variance:

[0059]

[0060] The observation equation is updated in this way, and the tracking model of the PLKF algorithm is obtained as follows:

[0061]

[0062] After calculating the tracking model parameters of the PLKF algorithm, the process of using PLKF for Kalman filtering is the same as the traditional linear Kalman filtering process. Figure 5 shown.

[0063] From the above derivation results of the PLKF tracking model, it can be seen that the observation equation of PLKF introduces observation noise into the observation matrix H, resulting in the correlation between the observation matrix H and the observation noise; in the ideal linear Kalman filter, the observation matrix is ​​uncorrelated with the observation noise, and this correlation will increase the filtering error of the algorithm. In this regard, the analysis of the deviation is as follows:

[0064]

[0065] In the formula,

[0066]

[0067] Therefore, the deviation at time k can be expressed as:

[0068]

[0069] Analyzing the deviation, we know that A k is the previous moment α k-1 The deviation propagation is the system error itself and cannot be eliminated; B k Indicates H k and the system noise w at the previous moment k-1 The correlation of C k Indicates H k With the observation noise η k The correlation of C cannot be ignored and is the main error generating term. k Compensating for the induced deviation can reduce the error of PLKF.

[0070] Ck It can be approximately calculated as:

[0071]

[0072] Known Substitute this into the new state estimate:

[0073]

[0074] In the formula, is the state vector of the traditional deviation compensation algorithm.

[0075] It can be seen from the above formula that the compensation of PLKF involves the observation noise variance. And when the system observation noise is known, the performance of the traditional bias-compensated PLKF algorithm is better than that of the EKF algorithm. However, when the observation noise variance is unknown, the deviation between the set system variance and the actual system variance is large, the compensation algorithm effect of PLKF is reduced, and the tracking performance after compensation is even worse than the compensation of the traditional EKF algorithm, and even weaker than the adaptive EKF algorithm. Therefore, in order to solve this problem, the RA-PLKF algorithm is proposed. The algorithm performs system variance adaptation on the traditional PLKF algorithm, thereby achieving adaptive improvement of bias compensation.

[0076] The new information d at time k k Defined as the actual observed value z of the filter k The difference between the predicted observed value and the

[0077]

[0078] In the formula, is the one-step prediction error.

[0079] Innovation Variance:

[0080]

[0081] According to the window estimation, the real-time estimated covariance of the new information can be obtained as follows:

[0082]

[0083] Where L is the sampling time width.

[0084] Combining equations (19) and (20), the observation noise variance is estimated:

[0085]

[0086] When the sampling width L is small, Since the sampling fluctuation may have a small value, the estimated noise variance may lose positive definiteness. In order to improve this problem, the following introduces an adaptive method based on residuals.

[0087] The residual r at time k k Defined as the actual observed value z of the filter k With the estimated observations The difference is:

[0088]

[0089] The residual variance is:

[0090]

[0091] According to the window estimation, the real-time estimated variance of the residual can be obtained as:

[0092]

[0093] Combining the classic Kalman filter principle, the filter gain is:

[0094]

[0095] At the same time, multiply H on the left k , put it into the formula and get:

[0096]

[0097] At the same time, multiply it by R on the right to get

[0098]

[0099] Substituting into formula (28) we get the R estimation formula based on residual error:

[0100]

[0101] Thus the following formula holds:

[0102]

[0103] In the formula, H k is the estimated observation matrix of PLKF, H θ,k is the estimated observation matrix obtained by expanding equation (2) according to the first-order Taylor formula. is the residual of the original azimuth observation.

[0104]

[0105] In the formula, is the azimuth angle estimated by the uncompensated PLKF algorithm,

[0106] Due to R k With the distance vector d kRelated to the observation variance, the target distance changes rapidly over time, so it is difficult to directly obtain the PLKF statistical residual at a certain moment through statistical methods. Therefore, the following method is proposed to transfer the original observation variance to the PLKF observation variance.

[0107] Known:

[0108]

[0109] According to formula (32-33), we can deduce:

[0110] H θ,k =-||d k ||H k (34)

[0111] According to formulas (30-31, 34), the observation error variance update formula of the improved PLKF is as follows:

[0112]

[0113] The new target state compensation formula is as follows:

[0114]

[0115] In the formula, is the compensation state vector based on residual adaptation.

[0116] Therefore, the RA-PLKF algorithm based on residual compensation is summarized as follows, and the process is as follows.

[0117] Status prediction:

[0118]

[0119] State covariance matrix prediction:

[0120]

[0121] Equivalent observation variance estimate:

[0122]

[0123] Kalman gain matrix:

[0124]

[0125] Status Update:

[0126]

[0127] State covariance matrix update:

[0128]

[0129] Deviation compensation:

[0130]

[0131] Observation position variance estimation:

[0132]

[0133] Due to the influence of noise fluctuations, when the observation variance is estimated at the beginning of tracking and the target has certain maneuvers during the course, RA-PLKF has the risk of over-compensation, resulting in lower accuracy after compensation than traditional PLKF.

[0134] Therefore, based on RA-PLKF, the SAM mechanism is introduced. When the system has a high probability of over-compensation, the original PLKF filtering result is used as the target filtering state value, which can reduce the possibility of filtering error and filtering divergence without increasing the amount of calculation. The main steps of the SAM criterion and SAM-RA-PLKF algorithm are as follows.

[0135] At time k, if ( It is recommended to take 3-5 times ), that is, at this moment, RA-PLKF is over-compensated, and the PLKF algorithm at the current moment does not perform deviation compensation.

[0136]

[0137] In the formula, is the azimuth angle estimated by the adaptive compensation PLKF algorithm, is the deviation threshold of RA-PLKF. The specific flow chart is as follows Figure 2 shown.

[0138] Simulation environment experiment Figure 3 shown.

[0139] The initial parameter settings are shown in Table 1.

[0140] Table 1. Initial state of the scene

[0141]

[0142] The patrol movement cycle of the observation platform is T = 800s, and the sampling interval is 1Hz. The movement trajectory is: 1-200s maintains a uniform speed at the initial point, 201-400s use two 100s uniform acceleration movements to turn around from corner 1 through the lowest point inflection point 2 to reach inflection point 3, 401-600s maintain a uniform speed, 601-800s use two 100s uniform acceleration movements to turn around from corner 4 through the highest point inflection point 5 to reach the end point, and the inflection point acceleration is shown in Table 2.

[0143] Table 2 Platform maneuvering acceleration

[0144]

[0145] Set the sampling interval T = 1s, the system process noise standard deviation δ0 = 0.05m / s 2 , the standard deviation of observation azimuth noise σ0 = 1 mrad.

[0146] Time-averaged RMSE:

[0147]

[0148] Where, t max is the total number of sampling points. RTAMS can be used to measure the comprehensive performance of the algorithm.

[0149] Set the sampling time to 600s and the initial observation noise to R5( 400 Monte Carlo experiments were conducted in the above experimental environment, divided into 10 groups, and each group conducted 50 experiments under the same target trajectory. The target trajectory was tracked as follows Figure 4 shown.

[0150] Table 3. Comparison of comprehensive performance of various algorithms

[0151]

[0152] In the patrol defense scenario of this experiment, from the perspective of comprehensive overall accuracy, the SAM-RA-PLKF algorithm has the best performance, slightly better than RA-PLKF, much better than IEKF and EKF, and the BC-PLKF algorithm has diverged and cannot track the target. Table 3 shows the time average root mean square error and the running time of a single sampling cycle of each algorithm, which can reflect the comprehensive performance of the algorithm. From the perspective of RTAMS in the position direction, SAM-RA-PLKF is 0.47% less than RA-PLKF, 10.57% less than IEKF, and 58.24% less than EKF. From the perspective of RTAMS in the speed direction, SAM-RA-PLKF is 0.142% more than RA-PLKF, 15.16% less than IEKF, and 38.30% less than EKF. The newly proposed algorithm maintains the time order of magnitude within the millisecond level, which does not affect the real-time performance of the algorithm. In summary, the newly proposed SAM-RA-PLKF algorithm has better comprehensive performance than the commonly used bearing-only tracking algorithm without affecting the real-time performance.

[0153] It should be pointed out that, according to the needs of implementation, the various steps / components described in this application can be split into more steps / components, and two or more steps / components or partial operations of steps / components can be combined into new steps / components to achieve the purpose of the present invention.

[0154] It will be easily understood by those skilled in the art that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. An adaptive bearing-only pseudo-linear Kalman filter tracking method, characterized in that: include: Obtain the target's observation information from sonar detection through ad hoc network communication; The residual is updated in real time using the observation information to correct the observation variance of the Kalman filter to locate the target; After successfully locating the target, the adaptive pseudo-linear Kalman filter is used to correct the target position in real time.

2. The method according to claim 1, characterized in that The method of using the observation information to update the residual in real time and correcting the observation variance of the Kalman filter includes: The observation information is filtered through PLKF, and the statistical deviation is calculated, and the deviation is compensated by PLKF; The residual variance is calculated and variance adaptation is performed to correct the observation variance of the Kalman filter.

3. The method according to claim 2, characterized in that The statistical residual variance is calculated and variance adaptation is performed to correct the observed variance of the Kalman filter, including: Depend on To predict the state, Perform state covariance matrix prediction; Depend on Equivalent observation variance estimation is performed by Get the Kalman gain matrix; Depend on Status update by Update the state covariance matrix; Depend on To compensate for the deviation, Estimation of observation position variance; Among them, x k-1 is the target state vector at time k-1, F is the state transfer matrix, is the predicted state vector, P k-1 is the system covariance matrix at time k-1, W k-1 is the observation error covariance at time k-1, is the one-step prediction system covariance matrix, d k is the vector from the platform position to the target position, is the estimated variance of the observation noise, is the estimated value of the observation noise variance, K k is the compensation gain, H k is the estimated observation matrix of PLKF, is the result of RA-PLKF algorithm, is the deviation compensation amount, P k / k is the system covariance matrix at time k, M is the intermediate variable driving matrix, is the predicted position of the platform at time k, r k is the residual at time k, is the deviation compensation of the original observation value of PLKF.

4. The method according to claim 3, characterized in that After calculating the statistical residual variance and performing variance adaptation to correct the observed variance of the Kalman filter, the method further includes: When over-compensation of filtering occurs, the SAM criterion is used for state estimation, and the PLKF filtering result is used as the target filtering state value.

5. The method according to claim 4, characterized in that When over-compensation of filtering occurs, the SAM criterion is used to perform state estimation, and the PLKF filtering result is used as the target filtering state value, including: At time k, if Depend on Perform state prediction, where is the azimuth angle estimated by the adaptive compensation PLKF algorithm, is the actual observation azimuth angle between the observation platform and the target relative to the positive half axis of the X-axis, is the deviation threshold of RA-PLKF, The target estimated position for the SAM-RAPLKF algorithm, Estimated position for the target of RAPLKF.

Images