A nonlinear moving target tracking method based on robust extended Kalman filter

Abstract

The present invention belongs to the field of UWB indoor positioning and target tracking. Aiming at the adaptability problem of target tracking methods to nonlinear motion in indoor UWB positioning scenarios, a nonlinear motion target tracking method based on robust extended Kalman filtering is proposed. Based on the use of Taylor approximation of nonlinear state transfer function and observation function, robust threshold control is performed on the error between estimated observation value and actual observation value to achieve adaptive correction of target state estimation. This can not only improve the robustness of the model to noise but also improve the accuracy of state correction, thereby obtaining a more accurate UWB positioning target trajectory.

Description

Technical Field

[0001] The present invention belongs to the field of UWB (Ultra Wide Band) indoor positioning and target tracking, and particularly relates to a nonlinear moving target tracking method based on robust extended Kalman filtering. Background Art

[0002] With the rapid development of modern infrastructure such as smart cities, smart factories, and smart medical care, indoor location services have become an emerging industry and are gradually gaining attention in daily life. Target tracking in UWB indoor positioning systems has become a research hotspot in recent years.

[0003] Tracking targets in UWB indoor positioning environments is subject to system and environmental noise interference, including occlusion by moving objects, clutter reflections, multipath interference, non-line-of-sight interference, and antenna errors. This can easily lead to target loss, sudden changes in trajectory, and abnormal results such as "target passing through walls." To address the noise interference issue with moving targets, the Kalman filter (KF) algorithm, a linear filter that can estimate the optimal state of a linear system, is widely used for target tracking due to its high computational efficiency and simple implementation.

[0004] Kalman filtering is primarily applicable to linear system models and is difficult to apply to nonlinear target tracking in indoor positioning applications. Therefore, nonlinear filtering algorithms such as the Extended Kalman Filter (EKF), Unscented Kalman Filter (UKF), Particle Filter, Adaptive Fading Kalman Filter, and Adaptive Fading Biased Extended Kalman Filter have been proposed. Among these, the parameter selection in the Unscented Kalman Filter relies to a certain extent on empirical values ​​and is prone to divergence when the system state dimension is too high. Adaptive Kalman filtering algorithms have high computational complexity. Particle filtering has poor real-time performance and long computation times, making it difficult to improve its filtering accuracy. The EKF algorithm, on the other hand, has strong real-time performance, is easy to implement, and is widely used, but its tracking accuracy is limited and its robustness is not high.

[0005] The above Kalman filter improvement algorithm excessively pursues adaptability to nonlinear targets, which not only produces a complex calculation process but also is prone to overfitting. Summary of the Invention

[0006] The present invention overcomes the deficiencies in the prior art and aims to solve the technical problem of providing a nonlinear moving target tracking method based on robust extended Kalman filtering to improve the moving target tracking accuracy.

[0007] In order to solve the above technical problems, the technical solution adopted by the present invention is: a nonlinear moving target tracking method based on robust extended Kalman filter, comprising the following steps:

[0008] S1. Determine the motion state matrix X k-1 , state transfer matrix A k-1 and the observation matrix H k ;

[0009] S2. Determine the modified control parameter α based on the system error r and the robust control parameter β;

[0010] S3. Get the initial motion state matrix X0 and the initial motion state covariance P0; let k = 1;

[0011] S4. Get the actual observed position Z of the moving target at time k k , according to Z k and the motion state matrix X at time k-1 k-1 , calculate the estimated state at time k Estimation error L k , Kalman filter gain K k , the updated state covariance matrix P k ;

[0012] S5. Determine the estimated error L k Whether the condition is met: ||L k ||>βr, if satisfied, proceed to step S6, if not satisfied, proceed to step S7;

[0013] S6. The estimated value X of the state at time k-1 k-1 and the covariance at time k Make corrections and then recalculate the estimated error L at time k k , estimated state Kalman filter gain K k , the updated state covariance matrix P k Then, return to step S5;

[0014] S7. Calculate the motion state matrix X at time k k , and output its first two components As the true position of the UWB positioning target, the calculation formula is:

[0015] S8. Set k=k+1, return to steps S4 to S7, and repeat the calculation.

[0016] In step S6, the state estimation value X at time k-1 is k-1 and the covariance at time k The correction method is:

[0017] S6.1: Estimated value X based on the state at time k-1 k-1 , state covariance at time k The state transition matrix A at time k-1 k-1 , the observation matrix H k , the covariance S of the estimation error at time k k , calculate the first revised state estimate The calculation formula is:

[0018]

[0019]

[0020] Among them, F k-1 represents the weighting matrix, (·) + To find the pseudo-inverse of the matrix, S k represents the covariance of the estimation error at time k, A k-1 represents the k-1 state transition matrix, H k represents the observation matrix;

[0021] S6.2: Calculate the second revised state estimate The calculation formula is:

[0022]

[0023]

[0024]

[0025] Among them, L k (1, 1) is the error between the actual observed X-axis coordinate and the predicted X-axis coordinate, L k (2, 1) is the error between the actual observed Y-axis coordinate and the predicted Y-axis coordinate, Represents the first revised state estimate The last three components of ; α represents the modified control parameter; and Represent the first revised state estimate The first two components of ;

[0026] S6.3: Calculate the modified state covariance The calculation formula is:

[0027]

[0028] S6.4: The second modified state estimate and the modified state covariance Assign values ​​to the state estimate X respectivelyk-1 and state covariance

[0029] Among them, the corrected control parameter α=β / r, r represents the system error, and β represents the robust control parameter.

[0030] In the steps S4 and S6, the estimated state Estimation error L k , Kalman filter gain K k The calculation formulas are:

[0031]

[0032]

[0033]

[0034] Among them, A k-1 represents the state transition matrix, H k represents the observation matrix, represents the state covariance at time k, S k represents the covariance of the estimation error, Z k Indicates the actual observed position value.

[0035] Covariance S of the estimation error k , the covariance matrix of the predicted state and the updated state covariance P k The calculation formula is:

[0036]

[0037]

[0038]

[0039] Among them, R k represents the covariance matrix of the observation noise, Q k Represents the covariance matrix of the state noise.

[0040] In step S1, the motion state matrix X at time k-1 k-1 Set to in, and are the horizontal and vertical coordinates of the target at time k-1, θ k-1 v is the angle between the tangent direction of the motion trajectory at time k-1 and the y-axis, k-1 is the speed at time k-1, a k-1 is the centripetal acceleration at moment k-1;

[0041] State transition matrix A k-1 Set to:

[0042]

[0043] In step S3, the first two components of the initial motion state X0 is the target initial value position, Z k is the observation value returned by the UWB positioning system, and the initial motion state covariance P0 = diag(1, 1, 1, 1, 1).

[0044] In step S4, the moving target observation position Z at time k k Estimated for the UWB positioning system.

[0045] Compared with the prior art, the present invention has the following beneficial effects:

[0046] The present invention provides a nonlinear moving target tracking method based on robust extended Kalman filtering. Based on the Taylor expansion approximation of the state transfer function and the observation function, a robust threshold feedback is established according to the upper bound of the error between the estimated observation value and the actual observation value, so as to guide the Kalman equation to correct the state estimate value and realize adaptive correction of the target state estimation. This method can not only improve the robustness of the model to noise but also improve the accuracy of state correction, thereby obtaining a more accurate UWB positioning target trajectory. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 A flowchart of a method for tracking a nonlinear moving target based on a robust extended Kalman filter provided by an embodiment of the present invention;

[0048] Figure 2 Schematic diagram of the trajectory tracking scenario of UWB indoor nonlinear moving target;

[0049] Figure 3 Schematic diagram of tracking results of different algorithms in three scenarios. DETAILED DESCRIPTION

[0050] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below. Obviously, the described embodiments are part of the embodiments of the present invention, not all the embodiments; based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0051] Taking the two-dimensional UWB indoor positioning scenario as an example, the method of the present invention is explained. In real applications, it can be directly extended to three-dimensional scenarios. Figure 2 As shown in the figure, it includes a group of UWB base stations (4 base stations in this embodiment), a nonlinear moving target (a moving target doing nonlinear motion in this embodiment) carrying a UWB positioning tag. The moving target sends a positioning signal to the surrounding area at regular intervals (every 1 second). The UWB positioning system estimates the position Z of the moving target at a certain moment based on the clock when the signal reaches each UWB base station through the TOA (Time of Arrival) positioning method. k (including noise).

[0052] like Figure 1 As shown, a nonlinear moving target tracking method based on robust extended Kalman filtering in an embodiment of the present invention includes the following steps:

[0053] S1. Determine the motion state matrix X k- 1. State transfer matrix A k-1 and the observation matrix H k .

[0054] The motion state matrix X at time k-1 set in this embodiment k-1 set up in, and are the horizontal and vertical coordinates of the target at time k-1, θ k-1 v is the angle between the tangent direction of the motion trajectory at time k-1 and the y-axis, k-1 is the speed at time k-1, a k-1 is the centripetal acceleration at moment k-1, which can adapt to a variety of nonlinear movement trajectories.

[0055] In this embodiment, the motion state matrix can be expressed as: k =f(X k-1 )+n k-1 , where f(·) represents the state transition function. n k-1 represents the state noise, and the motion state matrix can be expressed as:

[0056]

[0057]

[0058]

[0059]

[0060]

[0061] Among them, T represents the time difference between time k and time k-1, represents the additional noise of the motion speed in the motion state, represents the additional noise of acceleration in the motion state, is the state noise, Q k is the covariance matrix of the state noise, i.e. Q k =E(n k-1 n k-1 T ). Make a first-order Taylor approximation to the state transfer function to get the next moment state prediction value Among them A k-1 is the state transfer matrix, as shown in formula (6).

[0062]

[0063] Observation matrix In this example, the observation value is the position component in the motion state, and the observation function is linear, that is, the observation matrix

[0064] S2. Set the system error r and the robust control parameter β, and determine the corrected control parameter α.

[0065] In this embodiment, the system error r is set to the average value of the estimation error of the extended Kalman filter (EKF) [1] algorithm in the experimental environment. That is, in the application scenario, the target moves a certain distance (for example, 2 meters) along a straight line at a constant speed and the position of the moving target is estimated according to the subsequent formulas (7-12) and formula (19). Then, the average value of the estimation error is statistically calculated as the value of r.

[0066] The robust control parameter β is a parameter that determines the robustness of the control system. It is generally determined by dividing the upper bound (maximum value) of the estimation error by r. Because the upper bound is difficult to estimate, in this embodiment, an empirical parameter β is selected in the range of 0.1 to 0.3. Experiments have found that system robustness and performance only degrade when β is greater than 0.5. Therefore, the value of β can range from 0.1 to 0.5.

[0067] S3. Obtain the initial motion state matrix X0 and the initial motion state covariance P0; set k=1.

[0068] Specifically, in this embodiment, the first two components of X0 are is the target initial position, and the last three components are set to θ0 = 0, v0 = 1 m / s, and a0 = 0. P0 = diag(1, 1, 1, 1, 1).

[0069] S4. Get the observation value Z of the moving target at time k k , according to Z k , the estimated state value X at time k-1 k-1 Calculate the state prediction value at time k Estimation error L k, Kalman filter gain K k .

[0070] In step S4, the moving target observation value Z at time k k Parameter value returned by the UWB positioning system.

[0071] Predicted state value Estimation error L k , Kalman filter gain K k The calculation formulas are:

[0072]

[0073]

[0074]

[0075] Among them, A k-1 represents the state transition matrix, H k represents the observation matrix, Represents the covariance matrix of the predicted state, S k represents the covariance of the estimation error, Z k Indicates the actual observed position value.

[0076] Covariance S of the estimation error k , predicted state covariance Updated state covariance matrix P k The calculation formula is:

[0077]

[0078]

[0079]

[0080] Among them, R k Represents the covariance matrix of the observation noise, in this example, R k =diag(1,1),Q k Represents the covariance matrix of the state noise.

[0081] S5. Determine the estimated error L k Whether the condition is met: ||L k ||>βr, if satisfied, go to step S6, if not satisfied, go to step S7.

[0082] In this embodiment, if the estimation error is too large, the process proceeds to step S6 for correction.

[0083] S6. The estimated value X of the state at time k-1 k-1And the predicted state covariance matrix at time k Make corrections and then recalculate the estimated error L at time k k , the covariance S of the estimation error k , Kalman filter gain K k , the updated state covariance matrix P k Then, return to step S5.

[0084] Among them, the state estimate X at time k-1 is k-1 and the predicted state covariance at time k The specific method for making corrections includes the following steps:

[0085] S6.1: Estimated value X based on the state at time k-1 k-1 , the covariance matrix of the predicted state at time k The state transition matrix A at time k-1 k-1 , observation matrix K k , the covariance S of the estimation error at time k k , calculate the first revised state estimate The calculation formula is:

[0086]

[0087]

[0088] Among them, F k-1 represents the weighting matrix, (·) + To find the pseudo-inverse of the matrix, S k represents the covariance of the estimation error at time k, A k-1 represents the state transition matrix at time k-1, H k represents the observation matrix.

[0089] S6.2: Calculate the second revised state estimate The calculation formula is:

[0090]

[0091]

[0092]

[0093] Among them, L k (1, 1) is the error between the actual observed X-axis coordinate and the predicted X-axis coordinate, L k (2, 1) is the error between the actual observed Y-axis coordinate and the predicted Y-axis coordinate, Represents the first revised state estimate The last three components of ; α represents the modified control parameter; and Represent the first revised state estimate Specifically, in this embodiment, α=β / r.

[0094] S6.3: Calculate the modified state covariance The calculation formula is:

[0095]

[0096] S6.4: The second modified state estimate and the modified state covariance Assign values ​​to the state estimate X respectively k-1 and state covariance

[0097] In this embodiment, the state estimation value X k-1 and state covariance After correction, the estimated error L at time k is calculated using the above formulas (7) to (12): k , estimated state Kalman filter gain K k , the updated state covariance matrix P k Then return to step S5 to determine whether the estimated error exceeds the threshold, that is, the product of the system error r and the robust control parameter β.

[0098] S7. Calculate the motion state matrix Xk at time k and output its first two components (P xk , P yk ) is the real position of the UWB positioning target, and the calculation formula is:

[0099]

[0100] S8. Set k=k+1, return to steps S4 to S7, and repeat the calculation.

[0101]

Algorithm Verification

[0102] In order to verify the tracking performance of the proposed algorithm for nonlinear moving targets in UWB indoor positioning systems, three simulation scenarios were designed using moving targets on a two-dimensional plane as an example, each containing a nonlinear target trajectory of a circle, an ellipse, and a broken line. First, the measurement value of the target trajectory was calculated using the time of arrival (TOA) indoor positioning method, and then the proposed algorithm was combined with the extended Kalman filter (EKF) and the adaptive fading biased extended Kalman filter to obtain the target trajectory. [2](Adaptive Fading Biased Extended Kalman Filter), Unscented Kalman Filter [3] The measurement trajectory is tracked using Unscented Kalman Filter (UKF), and the root mean square error is used to compare the tracking performance of the above algorithms.

[0103] In the three experiments, the TOA positioning base station coordinates were (-10, 10), (10, 10), (10, -10), and (-10, -10), respectively, and the target initial velocity v = 1 m / s. Each scenario was designed as follows.

[0104] Scenario 1: The target starts from point A (18 / π, 7) and first moves in a uniform linear motion in the opposite direction of the y-axis to point B on the x-axis. Then, it moves in a circular motion with a radius of R = 18 / π around the origin of the coordinate system back to point B (18 / π, 0). Then, it moves in a uniform linear motion in the opposite direction of the y-axis to point C (18 / π, -7). The sampling interval is 1 second, and the number of sampling points determined by the target speed and the sampling interval is 76.

[0105] Scenario 2: The target trajectory is The ellipse is D(5,0)→E(0,-4)→F(-5,0)→G(0,4)→D(5,0), with a sampling interval of 0.1rad / s and a total of 90 points.

[0106] Scenario 3: The target trajectory is a rectangle H(-7, -5)→I(7, -5)→J(7, 5)→K(-7, 5)→H(-7, -5), with a sampling interval of 0.5s and a total of 100 sampling points.

[0107] The tracking results of TOA positioning measurement trajectory by the algorithm of the present invention, EKF, UKF and adaptive fading biased EKF in three scenarios are as follows: Figure 3 As shown. Figure 3 It can be seen from the figure that: compared with the TOA without filtering, the distance between the measured trajectory and the true trajectory is smaller, which once again verifies the effectiveness of filtering; the trajectories of EKF and UKF are closer to the true trajectory, which verifies the effectiveness of the improved algorithm; the present invention obtains the result closest to the true trajectory.

[0108] Use Root Mean Square Error (RMSE) to represent Figure 3 The error results of each algorithm and the true trajectory are shown in Table 1. As can be seen from Table 1, the average performance of the present invention is improved by 29.1%, 31.46%, and 18.85% respectively compared with UKF.

[0109] Table 1 Performance comparison of different algorithms

[0110]

[0111] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

[0112] References:

[0113] [1]X.Wang, Z.Xu, X.Gou, et al.Tracking a Maneuvering Target by MultipleSensors

[0114] Using Extended Kalman Filter With Nested Probabilistic-NumericalLinguisticInformation[J]. IEEE Transactions on Fuzzy Systems, 2020, 28(2): 346-360.

[0115] [2] Yan Chunman, Wu Songlun, Dong Junsong. Application of adaptive fading biased EKF filter in target tracking[J].

[0116] Journal of Technology, 2020, 33(02): 315-320.

[0117] [3]KV Yuen,YSLiu,WJYan.Estimation of time-varying noiseparameters forunscented Kalman filter[J].MECHANICAL SYSTEMS AND SIGNALPROCESSING.2022,180.

Claims

1. A nonlinear moving target tracking method based on robust extended Kalman filter, characterized in that: The following steps are involved: S1. Determine the motion state matrix , state transfer matrix and the observation matrix ; S2. According to the system error and robust control parameters , determine the modified control parameter α; S3. Get the initial motion state matrix and the initial motion state covariance ; Let k = 1; S4. Get the actual observed position of the moving target at time k ,according to and the motion state matrix at time k-1 , calculate the estimated state at time k , estimation error , Kalman filter gain , the updated state covariance matrix ; S5. Determine the estimated error Whether the conditions are met: If satisfied, go to step S6, if not satisfied, go to step S7; S6. The state estimation value at time k-1 according to the modified control parameter α and the covariance at time k Make corrections and then recalculate the estimated error at time k , estimated state , Kalman filter gain , the updated state covariance matrix Then, return to step S5; S7. Calculate the motion state matrix at time k , and output its first two components ( ) is the real position of the UWB positioning target, and the calculation formula is: ; S8. Set k = k + 1, return to steps S4 to S7, and repeat the calculation; In step S6, the state estimation value at time k-1 is and the covariance at time k The correction method is: S6.1: Based on the estimated state at time k-1 , state covariance at time k , the state transition matrix at time k-1 , the observation matrix , the covariance of the estimation error at time k , calculate the first revised state estimate , the calculation formula is: ； ； in, represents the weighting matrix, To find the pseudo-inverse of a matrix, represents the covariance of the estimation error at time k, represents the k-1 state transition matrix, Hk represents the observation matrix; S6.2: Calculate the second revised state estimate , the calculation formula is: in, is the error between the actual observed X-axis coordinate and the predicted X-axis coordinate, is the error between the actual observed Y-axis coordinate and the predicted Y-axis coordinate, Represents the first revised state estimate The last three components of represents the modified control parameter; and Represent the first revised state estimate The first two components of ; S6.3: Calculate the modified state covariance , the calculation formula is: ； S6.4: The second modified state estimate and the modified state covariance Assign values ​​to the state estimates respectively and state covariance ; Correction control parameters , r represents the systematic error, β represents the robust control parameters.

2. The method for tracking a nonlinear moving target based on a robust extended Kalman filter according to claim 1, wherein: In the steps S4 and S6, the estimated state , estimation error , Kalman filter gain The calculation formulas are: in, represents the state transition matrix, represents the observation matrix, represents the state covariance at time k, represents the covariance of the estimation error, Indicates the actual observed position value.

3. The method for tracking a nonlinear moving target based on a robust extended Kalman filter according to claim 1, wherein: Covariance of the estimation errors , the covariance matrix of the predicted state and the updated state covariance The calculation formula is: in, represents the covariance matrix of the observation noise, Represents the covariance matrix of the state noise.

4. The method for tracking a nonlinear moving target based on a robust extended Kalman filter according to claim 1, wherein: In step S1, the motion state matrix at time k-1 is Set to ,in, and are the horizontal and vertical coordinates of the target at time k-1, is the tangent direction of the motion trajectory at time k-1 and Axis reverse angle, is the speed at time k-1, is the centripetal acceleration at moment k-1; State transition matrix Set to: 。 5. The method for tracking a nonlinear moving target based on a robust extended Kalman filter according to claim 1, wherein: In step S3, the initial motion state The first two components of is the target initial value position, The observation value returned by the UWB positioning system, the initial motion state covariance .

6. The method for tracking a nonlinear moving target based on a robust extended Kalman filter according to claim 1, wherein: In step S4, the moving target observation position at time k Estimated for the UWB positioning system.

Images